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ABSTRACT 


This  paper  discusses  the  implementation  of  unilateral 
force  control  override  of  rate  control  of  a  PUMA  560  robot 
manipulator  in  three  degrees  of  freedom.  A  control  system  is 
developed  utilizing  the  necessary  sensors,  hardware,  and 
software  interface  to  enable  one  to  operate  the  manipulator  in 
rate  control  with  unilateral  force  control  override.  A  review 
of  the  theory  behind  such  a  controller  is  conducted  and 
stability  issues  addressed.  A  comparison  of  experimental 
results  with  theoretical  results  is  conducted  and  a  simple 
program  is  developed  to  simulate  the  manipulator's  response. 
These  simulation  results  are  compared  to  the  experimental 
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I .  INTRODUCTION 


Force  control  of  robot  manipulators  has  received  much 
attention  during  recent  years.  This  is  due  in  part  to  the 
growing  interest  in  the  use  of  robot  systems  to  interact  with 
undefined  environments  in  carrying  out  tasks.  The  ability  to 
measure  and  control  the  forces  between  a  manipulator  and  the 
environment  provides  greater  flexibility  to  the  system  when 
such  interactions  with  the  surroundings  are  required  or 
desired.  Traditional  manipulator  position  control  methods 
present  limitations  to  a  robot  system  in  force  control.  If 
rate  and  position  control  of  a  robot  are  used  exclusively, 
there  is  a  need  for  thorough  knowledge  of  the  surroundings  and 
extreme  precision  in  the  control  of  the  manipulator's 
position.  The  ability  for  the  robot  to  interact  with  the 
environment  without  causing  damage  to  the  manipulator  or 
surrounding  objects  is  dependent  on  the  stiffness  of  the 
manipulator  and  the  environment.  If  the  stiffness  of  each  of 
these  is  high,  damage  is  likely  to  occur  when  the  robot's  end 
effector  comes  in  contact  with  an  obstruction  as  it  continues 
to  try  and  reach  the  desired  position  or  maintain  the  desired 
rate. 

The  optimum  characteristics  of  each  type  of  controller 
differ  considerably.  Most  robot  systems  in  use  utilize 
position  control  and  are  very  stiff  in  order  to  provide 
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precision  in  positioning  and  speed  in  their  motions.  On  the 
other  hand,  if  one  desires  precise  control  cf  the  forces  a 
manipulator  applies  to  an  object,  a  manipulator  of  low 
stiffness  would  be  advantageous.  This  allows  a  reasonable 
response  speed  to  be  maintained  and  lower  contact  forces  to 
develop  as  a  result  of  position  errors.  This  conflicting 
requirement  in  stiffness  for  the  two  control  methods  presents 
a  difficult  design  problem,  where  a  compromise  must  be  made. 

Several  applications  can  make  use  of  this  type  of  control 
approach.  Almost  any  use  of  tele-operated  robotics  systems 
and  autonomous  vehicles  can  benefit  from  this  control 
structure.  The  space  shuttle  RMS  is  currently  operated  using 
rate  control  which  limits  its  capabilities.  With  the  addition 
of  a  force  override  to  the  rate  control  system  it  could 
perform  several  tasks  which  currently  require  astronauts.  The 
space  shuttle  RMS  could  be  utilized  to  retrieve  a  satellite 
with  the  application  of  this  control  system  rather  than 
requiring  several  astronauts  to  perform  a  space  walk.  Other 
uses  include  assembly  tasks  of  components  in  hazardous  or 
sensitive  environments  in  which  human  interaction  is  not 
directly  possible  or  desired.  This  includes  work  on  high 
voltage  electrical  systems,  assembly  of  parts  in  a  controlled 
atmosphere,  and  assembly  or  disassembly  tasks  in  the  nuclear 
industry . 

The  goal  of  this  research  is  to  extend  the  ability  to 
control  a  manipulator  using  force  override  rate  control  to 
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three  degrees  of  freedom  (DOF)  from  a  previously  demonstrated 
single  DOF. [Ref.  1]  Control  of  forces  in  three 
cartesian  space  directions  will  be  performed  utilizing  a  PUMA 
560  manipulator  arm.  The  control  system  will  be  developed  and 
analyzed  for  stability  and  performance.  The  system  will  also 
be  simulated  with  a  one  DOF  model  and  compared  with 
theoretical  results  of  a  simple  one  DOF  linear  model. 
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II 


THEORY 


A.  ROBOT  CONTROL 

A  review  of  the  basic  manipulator  control  problem  will  be 
conducted  before  developing  a  force  override  rate  controller 
for  a  PUMA  560  manipulator.  This  review  will  then  be  expanded 
to  include  a  discussion  of  force  control  of  manipulators  in 
the  next  section  and  finally,  the  development  of  a  force 
override  rate  controller  in  a  third  section. 

1 .  Control  Methods 

Controlling  a  robot  manipulator  can  be  categorized 
into  three  general  areas.  These  include  joint  motion  control, 
resolved  motion  control,  and  adaptive  control.  Each  of  these 
methods  are  currently  utilized  in  control  systems  for  existing 
manipulators  and  each  has  advantages  and  disadvantages.  Joint 
motion  control  concerns  the  most  elementary  aspect  of 
controlling  a  manipulator  in  that  it  deals  with  the  control  of 
each  individual  joint.  All  control  strategies  must 
incorporate  this  aspect  of  robot  control  in  some  way. 
Resolved  motion  control  involves  developing  control  algorithms 
based  on  a  cartesian  or  other  useful  coordinate  system  more 
easily  related  to  by  an  operator  in  a  given  situation. 
Adaptive  control  utilizes  some  type  of  model  of  the 
manipulator  as  well  as  any  environmental  constraints  to 
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control  the  manipulator.  Each  of  these  methods  are  discussed 
below. 

2.  Joint  Space  Control 

Until  recent  years  most  applications  of  robot  controls 
have  used  controllers  that  treated  each  joint  of  a  multi-joint 
manipulator  as  a  separate,  independent  system.  Fu,  Gonzalez, 
and  Lee  develop  the  equations  of  motion  and  transfer  function 
for  a  linearized  model  of  a  single  revolute  joint. 
[Ref.  2]  Figure  2.1  provides  a  relatively  complete 
block  diagram  of  a  single  electric  motor  driven  joint.  The 
block  diagram  notation  is  defined  as  follows: 

J  is  the  effective  inertia  of  the  mechanical  joint  and 
servo  motor  combined. 

La  is  the  servo  motor  inductance. 

ra  is  the  servo  motor  resistance. 


Figure  2.1.  Single  Joint  Control  Block  Diagram. 
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Dr  is  the  viscous  damping  in  the  joint. 

n  is  the  gear  ratio  between  the  servo  motor  rotation  angle 
and  the  joint  angle. 

D(s)  is  any  external  disturbances  applied  to  the  system. 

Gc  is  the  position  controller  transfer  function. 

Ka  is  the  proportionality  constant  between  applied  voltage 
and  output  torque  for  the  servo  motor. 

Kb  is  the  back  electromotive  force  of  the  servo  motor 
resulting  from  it's  motion. 

0DES(s)  is  the  desired  joint  angle. 

0(s)  is  the  actual  joint  angle. 

r(s)  is  the  torque  applied  by  the  joint  motor. 


The  following  equation  gives  the  closed  loop  transfer  function 
for  this  system. 

Q  (s)  _ _ GcKa _ 

&DES  <  s>  JLaS 3  +  (  JRA+DpLA)  S 2  +  ( RaD+KaKb)  S+GcKa 


Gc  for  a  PID  position  controller  can  be  represented  by 

Gc=Kp+KyS+—  (2.2) 

s 

where  Kp  is  a  proportional  gain  constant,  Ky  is  a  derivative 
gain  constant  and  Kj  is  an  integral  gain  constant.  Assuming 
the  electrical  time  constant  of  the  servo  motor  is  small  the 
term  La  can  be  neglected,  leaving 

= _ GcKA _  (2.3) 

®des(s'>  JRas2+  ( RaDr+KaKb)  S + GcKa 
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Substituting  in  the  terms  for  Gc  and  performing  some  algebra 
results  in 


e<s>  _ 


(KyKA/JRA)  S+KpKA/JRA+ 


k^a/jra 


*dbs(S)  S2+  ( RaDr+KaKb+KvKa )  /  JRa)  S+KpKjJRs 


(2.4) 


This  is  a  third  order  system  with  Gc  containing  all  three 
control  terms.  If  Kj  is  small,  as  is  often  the  case  in 
practice,  the  system  closely  follows  the  response  of  a  second 
order  system  represented  by 

9(g)  _ _ (KyKA/  JR A)  s  +KpKA/  JRa 

&DES  (s)  s2*((RaDr+KaKb+KvKa) /JRa)  S+KpKA/JRA  1  *  ’ 

Kp  and  Kv  can  be  selected  to  provide  the  desired  response  which 
is  usually  designed  to  be  critically  damped  or  slightly 
underdamped  with  a  natural  frequency  considerably  lower  than 
the  manipulator's  structural  natural  frequency. 

In  a  multi- joint  robot  D(s)  includes  gravitational 
loading  as  well  as  the  reaction  torques  resulting  from  the 
motion  and  drive  torques  of  the  rest  of  the  manipulator's 
joints.  These  disturbances  are  a  complex  function  of  the 
manipulator's  position  and  motion.  Additional  disturbances 
would  result  from  any  constraints  applied  to  the  end  effector 
by  adding  a  payload  or  by  an  interaction  with  an  environmental 
constraint,  as  is  the  case  in  a  force  control  application. 
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Most  current  manipulator  applications  utilize  constant 
values  for  Kp,  Kv,  and  Kj  in  the  joint  servo  controller.  This 
is  the  case  for  the  PUMA  560  manipulator.  This  control  scheme 
results  in  a  varying  response  of  the  manipulator  with  changes 
in  manipulator's  position,  motion  and  loading. 

Several  algorithms  have  been  developed  that  calculate 
the  expected  disturbances  and  compensates  for  them  in  the 
manipulator's  control  scheme.  One  such  scheme,  the  computed 
torgue  technigue,  calculates  the  reguired  torgues  needed  at 
each  joint,  taking  into  account  the  interactions  between  the 
joints  and  with  an  added  inertial  load  or  other  constraint  at 
the  end  effector.  This  calculated  torgue  is  then  utilized  in 
the  controller  to  drive  the  motions  of  the  manipulator  by 
using  feedforward  terms  or  some  other  control  method.  Craig 
develops  the  Newton-Euler  and  Lagranian  formulations  of  the 
computed  torgue  technique  to  establish  the  necessary  drive 
torques  of  each  joint  for  a  desired  motion. [Ref.  3] 
Although  this  type  of  control  theoretically  provides  more 
uniform  response,  computational  complexity  makes  this  method 
difficult  to  achieve  in  practice.  This  control  scheme  also 
has  the  drawback  of  needing  exact  information  concerning  the 
manipulator's  characteristics  and  environmental  constraints  in 
formulating  drive  torques  for  the  joints.  This  is  virtually 
impossible  in  many  applications  and  the  response  still  depends 
on  the  use  of  feedback  control  as  formulated  earlier. 
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An  adaptive  control  scheme  provides  an  optional 
control  method  without  the  drawbacks  just  discussed.  This 
method  is  briefly  described  below. 

3 .  Adaptive  Control 

The  need  for  accurate  models  of  the  manipulator 
dynamics  and  environmental  constraints  often  limit  the  ability 
to  use  control  schemes  like  the  computed  torque  technique. 
One  solution  to  this  problem  is  to  use  an  adaptive  control 
algorithm.  Adaptive  controls  schemes  compare  the  response  of 
the  actual  manipulator  to  a  reference  model  and  the  resulting 
difference  between  the  two  responses  is  in  some  way  used  to 
vary  the  feedback  gains  in  the  manipulator  joint  control 
loops.  This  essentially  causes  the  manipulator  dynamics  to  be 
modified  until  the  manipulator  dynamics  match  the  reference 
model . 

There  are  several  methods  developed  to  actually  apply 
this  method  in  practice  and  Fu,  Gonzalez  and  Lee  presents  four 
of  these. [Ref.  4]  The  ability  to  change  the  feedback 
gains  of  the  joint  servo  controllers  enables  consistent 
response  of  the  manipulator  in  a  wide  range  of  motions, 
payload  conditions  and  constraints  of  the  end  effector  motion. 
This  type  of  control  also  requires  considerable  computational 
power  in  the  robot's  controller  in  order  to  perform  the 
necessary  algorithm  that  adjusts  the  feedback  gains  for  the 
proper  response.  The  PUMA  560 's  control  algorithm  does  not 
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allow  for  this  type  of  control  and  employing  such  a  control 
strategy  for  the  PUMA  560  requires  essentially  replacing  the 
existing  PUMA  controller  with  a  completely  different  system. 

4.  Resolved  Motion  Control 

It  is  most  convenient  for  an  operator  to  control  a 
manipulator  in  terms  of  a  cartesian  space  reference  frame.  A 
reference  frame  with  coordinates  associated  with  the  end 
effector,  referred  to  as  the  tool  frame,  is  most  often 
utilized  while  a  base  or  world  frame,  defined  with  respect  to 
a  stationary  point  in  relation  to  the  base  of  the  robot,  is 
sometimes  used.  The  desired  path  defined  in  cartesian  space 
must  be  related  to  the  required  motions  of  the  manipulator's 
joints  to  achieve  the  motion.  Craig  [Ref.  5]  briefly 
discusses  the  concepts  of  resolved  motion  control  while  Fu, 
Gonzalez,  and  Lee  [Ref.  6]  go  into  a  more  complete 
development  of  the  control  formulation.  The  control  algorithm 
relies  on  the  following  equations  which  relate  the  coordinate 
kinematics  of  the  cartesian  reference  frame  to  those  of  the 
manipulator  joint  motion. 


®DES  (  t)  =  T  1  (  X DBS  (  t )  ) 

(2.6) 

fcpss(t)  =iv-1(e)4BS(t) 

(2.7) 

( t)  =N'1  (©)  XDES(t)  +N- 1  (0)  XDES(  t) 

(2.8) 
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where: 


N  is  the  Jacobian  matrix  for  the  manipulator. 

T  is  the  coordinate  transformation  from  joint  space  to 
cartesian  space. 

XDES  is  the  desired  end  effector  position  defined  in 
cartesian  space. 

0OES  is  the  desired  manipulator  joint  positions. 

These  equations  indicate  the  computational  complexity 
of  this  control  method.  Most  existing  manipulators  controlled 
in  cartesian  space  do  not  fully  utilize  these  equations  in 
their  control  scheme.  The  control  system  is  often  limited  to 
performing  the  necessary  computations  to  provide  the  inverse 
kinematics  relating  the  joint  positions  to  the  cartesian  space 
position,  although  resolved  motion  rate  control  and 
acceleration  control  which  use  the  remaining  two  equations  are 
possible.  Fu,  Gonzalez,  and  Lee  also  present  the  basic 
principles  behind  resolved  motion  force  control  which  utilizes 
the  following  relationship  to  relate  forces  and  moments  to  be 
applied  by  the  manipulator's  end  effector,  FDES(t),  to  the 
required  joint  torques,  r(t).[Ref.  7] 

x  ( t)  =Nt(&)  F(  t)  (2*9) 

The  PUMA  560  allows  for  two  general  motion  control 
schemes.  In  the  first  method  a  motion  command  is  related  to 
the  necessary  joint  position  changes  and  each  joint  then  moves 
as  necessary  to  achieve  the  required  new  position  in  the 
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specified  time  allowed  for  the  complete  move.  This  is  done 
independently  of  the  other  joints.  This  is  referred  to  as 
joint-interpolated  motion  and  is  performed  utilizing  the  MOVE 
command  in  the  PUMA  560 's  VAL  program  language.  The  main 
disadvantage  of  this  type  of  method  is  that  it  does  not 
actually  move  the  manipulator  in  a  straight  path  from  the 
initial  position  to  the  final  position.  The  other  control 
algorithm,  referred  to  as  straight-line  motion  results  in  the 
end  effector  moving  in  a  straight-line  path  from  the  initial 
position  to  the  final  position.  This  is  done  by  breaking  the 
complete  move  into  incremental  motion  segments .  Each  of  these 
segments  is  converted  to  the  necessary  incremental  positions 
each  joint  must  attain.  This  scheme  uses  the  MOVES  command  in 
the  VAL  program  language  and  is  used  when  the  specific 
trajectory  of  the  end  effector  is  important.  The  drawback  of 
this  scheme  is  that  greater  computational  complexity  is 
involved  which  limits  the  speed  of  the  manipulator's  response. 
Further  details  of  the  motion  control  of  the  PUMA  560  will  be 
considered  in  a  later  section. 

B.  FORCE  CONTROL 

1 .  Control  Methods 

Many  force  control  strategies  have  been  developed  over 
the  years.  Whitney  discusses  several  of  these  control 
strategies  from  an  historical  perspective. [Ref •  8] 
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Performance  and  system  response  differ  for  each  method  making 
each  better  suited  for  varying  applications. 

Force  control  strategies  can  be  categorized  into 
stiffness  methods,  damping  methods  ,  impedance  methods, 
explicit  force  methods,  and  compliance  methods.  Each  of  these 
methods  are  discussed  briefly  below  and  evaluated  as  to 
applicability  toward  force  override  rate  control  of  the  PUMA 
manipulator . 

a.  Stiffness  Control 

Figure  2.2  shows  a  simple  block  diagram  of  a 
general  model  for  stiffness  control.  The  electrical  time 
constant  of  the  servo  motor  is  neglected  and  the  manipulator 


Figure  2.2.  Stiffness  Control  Block  Diagram. 
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is  described  by  an  inertial  load,  J,  and  a  viscous  damping 
term,  D.  KFS  is  a  force  feedback  stiffness  gain,  KE  represents 
the  overall  stiffness  at  the  end  effector/environment 
junction,  XENV  is  the  position  of  the  environmental  constraint, 
and  all  other  terms  are  as  defined  previously. 

Forces  sensed  at  the  end  effector  are  multiplied  by 
a  gain  matrix,  KFS,  which  results  in  a  change  in  the  desired 
position.  The  PUMA  control  architecture  and  programming 
language  lends  itself  well  to  this  type  of  control  as  the 
programming  commands  of  the  PUMA's  VAL  language  are  based 
primarily  on  position  control  of  the  manipulator  joints  to 
achieve  the  desired  end  effector  position.  To  utilize  this 
method  for  the  PUMA  560,  force  sensors  attached  to  the  end 
effector  provide  the  necessary  feedback  which  is  then  used  to 
update  the  position  commands  for  the  manipulator.  This  is 
essentially  part  of  the  method  used  in  the  force  override  rate 
controller  to  be  described  in  detail  in  a  later  section. 
b.  Damping  Control 

Damping  control  is  very  similar  to  stiffness 
control  but  instead  of  utilizing  force  feedback  to  change  the 
position  command,  the  force  feedback  is  used  to  change  the 
commanded  velocity  of  the  manipulator.  Figure  2.3  provides  a 
block  diagram  of  damping  control  with  the  additional  term  KFD 
being  a  force  feedback  damping  term.  Forces  sensed  at  the  end 
effector  are  multiplied  by  Kpp  which  provides  an  input  to  the 
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commanded  velocity.  This  method  cannot  be  directly  applied  to 
the  PUMA'S  control  structure  since  the  PUMA'S  programming 
structure  relies  primarily  on  position  commands  for  control. 
However,  this  method  can  be  adapted  to  the  control  structure 
to  provide  a  form  of  rate  control  to  the  PUMA. 


Figure  2.3.  Damping  Control  Block  Diagram. 

Within  the  PUMA'S  programming  capabilities  is  the 
ability  to  specify  a  speed  at  which  a  particular  position 
order  is  carried  out.  This  feature  of  the  PUMA'S  control 
structure  enables  rate  control  of  the  manipulator  to  be 
performed  in  conjunction  with  position  control.  Details  of 
this  control  structure  are  discussed  in  the  next  section. 
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c.  Impedance  Control 

Impedance  control  is  a  combination  of  both 
stiffness  control  and  damping  control  within  one  controller. 
Figure  2.4  provides  a  block  diagram  of  such  a  controller. 


Figure  2.4.  Impedance  Control  Block  Diagram. 

This  combination  results  in  PID  force  control  which  provides 
flexibility  in  controlling  the  manipulator's  response.  The 
force  override  rate  controller  to  be  developed  in  this  paper 
is  very  similar  to  this  form  of  control  although  it  is  not 
possible  to  implement  this  type  of  control  exactly  using  the 
original  PUMA  control  architecture  and  programming 
capabilities . 
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d.  Explicit  force  control 

Figure  2.5  shows  a  block  diagram  of  an  explicit 
force  control  method.  In  this  method  the  desired  forces  are 
direct  inputs  to  the  control  algorithm  which  are  compared  to 


Figure  2.5.  Explicit  Force  Control  Block  Diagram. 

the  forces  fed  back  from  a  sensor  located  at  the  end  effector. 
The  force  error  eF,  is  converted  into  joint  space  and 
multiplied  by  a  gain,  KF,  which  produces  the  torque  developed 
in  each  of  the  manipulator's  joint  servos.  This  is 
essentially  a  form  of  acceleration  control.  In  the  absence  of 
any  constraints,  the  manipulator  will  accelerate  at  a  rate 
proportional  to  the  force  error  between  the  desired  force  and 
the  actual  force  developed  at  the  end  effector.  This  method 
cannot  be  directly  implemented  to  control  the  PUMA  as 
described,  in  which  the  joint  servos  are  driven  directly  by 
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force  errors.  However,  a  control  program  could  be  written  to 
indirectly  apply  this  concept  by  converting  the  force  error 
into  an  acceleration  command,  which  is  then  integrated  into 
rate  and  position  commands  that  can  be  used  to  control  the 
manipulator  using  the  existing  position  control  structure. 
When  the  manipulator  is  constrained,  this  method  would  provide 
a  good  force  response  with  no  steady  state  error.  In 
unconstrained  motion  though,  the  position  and  rate  of  the 
robot  is  much  more  difficult  to  control.  A  force  error  in 
free  space  would  give  rise  to  an  acceleration  of  the 
manipulator.  To  actually  stop  the  manipulator  at  a  desired 
position  would  be  extremely  difficult  since  it  requires 
considerable  operator  input  to  attain  the  necessary  desired 
force  input  to  establish  a  zero  commanded  velocity.  This  does 
not  even  consider  getting  the  end  effector  to  stop  at  the 
desired  location.  Having  said  this,  some  experimental 
evaluation  of  this  method  is  performed  in  an  effort  to 
validate  these  theoretical  deductions, 
e.  Compliance  Control 

Figure  2.6  describes  an  example  of  compliance 
control.  The  algorithm  is  very  similar  to  the  stiffness 
control  in  that  the  force  feedback  is  related  to  a  change  in 
the  ordered  position.  In  this  case  though,  the  rate  of  change 
of  the  forces  developed  at  the  end  effector  are  also  taken 
into  account  as  shown  by  the  term  Ks+KDs  in  the  feedback  loop 
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Figure  2.6.  Compliance  Control  Block  Diagram. 

where  Ks  and  K0  represent  a  desired  manipulator  compliance. 
Ishikawa,  Sawada,  Kawase,  and  Takata  develop  this  type  of 
controller  for  the  PUMA  560  manipulator  with  variable  position 
error  gain,  Kp,  in  the  joint  servo  controllers. 
[Ref.  9]  This  variable  gain  requires  essentially 
replacing  the  original  computer  hardware  and  software  of  the 
PUMA  with  three  computer  processors  that  perform  all  the 
computational  tasks,  and  interface  with  the  manipulator's 
joint  servos  to  make  the  control  system  possible.  This 
alteration  provides  the  computational  speed  necessary  to 
prevent  unacceptable  delays  in  implementing  the  control 
algorithm.  The  added  feature  of  variable  gain  in  the  position 
controller  enables  stability  to  be  maintained  for  virtually 
any  constraint  condition  and  desired  manipulator  compliance 


parameters.  While  this  provides  more  flexibility  in  the 
constraints  the  manipulator  can  handle,  it  still  has  the 
problem  of  compromising  responsiveness  for  the  sake  of 
retaining  stability,  which  is  common  to  most  force  control 
strategies . 

C.  UNILATERAL  FORCE  OVERRIDE  OF  RATE  CONTROL 
1 .  Description 

Figure  2.7  provides  a  block  diagram  for  a  simple  model 
of  a  proposed  force  override  rate  controller  for  the  PUMA  560 
in  one  DOF.  The  previous  definitions  for  the  variables 


Figure  2.7.  Force  Override/Rate  Control  Block  Diagram. 

continues  to  apply  with  KFP  and  KFV  representing  force  error 
position  and  velocity  gains,  respectively.  The  controller 
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consists  of  essentially  two  control  loops.  The  inner  loop 
contains  the  existing  control  system  of  the  PUMA  position 
controller  and  the  outer  loop  provides  position/rate  control 
commands  to  the  inner  loop  based  on  force  errors.  Figure  2.8 
provides  a  description  of  the  joystick  and  end  effector  used 
in  this  control  system.  Forces  sensed  at  the  joystick  and  the 
end  effector  are  compared  to  provide  a  force  error.  This 
force  error  is  then  multiplied  by  position  and  rate  gains  to 
provide  position  and  rate  signals  to  the  manipulator's 
controller.  The  PUMA  560 's  VAL  programming  language  does  not 
allow  one  to  order  a  rate  command  without  any  position 
command,  but  does  allow  a  rate  command  in  conjunction  with  a 
particular  position  command.  If  no  rate  control  command  is 
provided  with  a  position  command  the  manipulator  is  programmed 
to  carry  out  the  position  command  at  a  speed  specified  in  the 
within  the  control  architecture.  This  is  one  reason  for  &DES 
being  dependent  on  X0ES  as  shown  in  the  block  diagram.  This 
linear  model  of  the  block  diagram  does  not  model  the  complete 
control  structure  of  the  PUMA  560  but  provides  some  insight 
into  the  expected  behavior.  A  simulation  is  developed  in  a 
later  section  which  more  accurately  emulates  the  control 
structure  of  the  PUMA  560,  taking  into  account  time  delays  in 
the  control  program. 
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End  Effector 


Figure  2.8.  Joystick  and  End  Effector. 

2 .  Analysis 

Simplifying  the  system  by  assuming  a  value  of  zero  for 
Kfv  results  in  the  following  closed  loop  transfer  function  with 
respect  to  forces: 


F(S) 

F DBS ^  S) 


KFPKpKE + KFPKE — 


JS  2  +DS + 


(2.10) 
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Defining  the  following  variables  relating  the  environmental 
stiffness  to  the  position  feedback  gain  and  the  force  error 
gain  to  the  environmental  stiffness. 

Kfp=-§-  (2.11)  KB=aKp  (2.12) 

Kg, 


Making  the  necessary  substitutions  the  transfer  function 
becomes 

F(s)  _ _ t(K,8+Kx) _ 

FDBS(s)  i7S3+I>S2+  (a+f)+l)  JCpS+JCj  ( P +1 ) 


Kj  is  assumed  to  be  small  the  system  has  a  second  order 
transfer  function  of 


F(s)  _ _ _ 

fdes(s )  s2+2Cwns+(a+P+l)«fl 

with  the  following  definitions 


(2.14) 


(2.15) 


2  (2.16) 


Ishikawa,  Sawada,  Kawase,  and  Takata  experimentally 
determined  values  for  the  characteristics  of  the  PUMA  560  in 
their  development  of  a  compliance  controller  and  these  values 
are  as  follows:  [Ref.  10] 

Kp  =  85000  N/ro 
u  =  125.7  rad/s 

{  »  1.0  (Assumes  critical  damping) 
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Figure  2.9  provxdes  a  Bode  plot  of  the  system  using 
values  of  a  =  0.015  and  0  =  1.0.  This  value  of  a  closely 
approximates  the  experimental  values  for  Kp  and  KE  while  the 
value  of  0  is  arbitrarily  set  in  the  control  program  and  can 
be  changed.  With  simple  proportional  control  of  the  force 
error,  a  steady  state  error  in  the  actual  applied  force 
results  from  a  simple  step  input. 
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Figure  2.9.  Bode  Plot  of  Force  Override/Rate  Control. 


From  the  transfer  function  this  error  to  a  step  input  can  be 
expressed  by 
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The  steady  state  error  and  speed  of  response  are  directly 
related  to  the  factors  a  and  3,  with  3  having  the  greater 
impact.  A  low  value  for  a,  corresponding  to  low  overall 
stiffness  of  the  system,  is  desired  and  a  large  value  for  3/ 
corresponding  to  a  large  value  of  KFP  ,  results  in  a  better 
response.  The  value  of  3  is  expected  to  help  predict  the 
stability  of  the  system  when  computational  delays  are 
introduced  to  the  system. 

Replacing  KFp  with  KFP+KFI/s  results  in  proportional 
plus  integral  force  control.  This  leads  to  a  transfer  function 
of 

F(s)  _ P Kps + g KFIKP _  ^  jgj 

fdes^s )  Js3+£>s2+(a+p+l)  KpS+aKFIKp 


This  eliminates  the  steady  state  error  in  force  for  a  step 
input  but  results  in  difficult  control  of  the  manipulator  in 
free  space.  Giving  KFP  a  value  of  zero  results  in  a  control 
structure  essentially  equivalent  to  the  explicit  force  control 
method  described  previously.  A  force  error  will  produce  an 
acceleration  of  the  manipulator  rather  than  the  desired  rate 
control.  This  results  in  difficulty  in  controlling  the 
manipulator's  motion  in  free  space.  If  contact  between  the 
manipulator  and  a  constraint  could  be  detected  then  a  control 
algorithm  could  change  the  value  of  KFI  from  zero  in  free  space 
while  in  rate  control,  to  some  positive  value  when  in  contact 
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with  a  constraint  to  enable  reducing  the  steady  state  force 
error.  In  the  current  application  only  force  error  signals 
are  used  as  inputs  to  the  control  algorithm  and  the  operator's 
visual  observation  is  relied  upon  to  determine  when  contact  is 
made. 

This  analysis  has  been  concerned  with  a  single  DOF 
model  of  the  six  DOF  robot  system.  This  enables  a  simple 
analysis  of  the  system  but  results  in  inaccuracies.  Eppinger 
and  Seering  perform  an  analysis  of  the  effects  of  these 
manipulator  model  simplifications  on  the  response  and 
stability  of  force  control . [Ref .  11]  Not  taking  into 
account  the  complete  system  dynamics  results  in  an  inherently 
stable  system  model  which  when  fully  modelled  can  show 
unstable  behavior. 
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Ill .  PRELIMINARY  WORK 


A.  SINGLE  DOF  HYDRAULIC  SYSTEM 

The  idea  of  controlling  a  manipulator  such  as  the  Space 
Shuttle  RMS  in  rate  control  with  force  override  stems  from  a 
desire  to  be  able  to  use  rate  control  to  properly  position  the 
manipulator  with  respect  to  an  object  and  then  automatically 
transition  to  force  control  once  contact  is  made.  This  would 
be  particularly  useful  if  one  is  working  with  a  moving  object 
such  as  would  be  the  case  with  the  Space  Shuttle  RMS 
interacting  with  a  satellite.  This  concept  of  force  override 
of  the  normal  rate  control  of  a  manipulator  has  been  tested 
using  a  single  DOF  hydraulic  system  [Ref.  12]. 

1 .  Description 

Figure  3 . 1  provides  a  general  description  of  the 
hydraulic  force  control  system. [Ref.  13]  An  electro- 
hydraulic  servo  valve  controls  the  fluid  flow  to  a  cylinder 
whose  piston  rod,  representing  an  end  effector,  provides  a 
linear  displacement  proportional  to  the  fluid  flow  rate.  A 
force  sensor,  consisting  of  strain  gages  mounted  on  a 
cantilever  beam  assembly,  is  attached  to  the  end  effector  and 
measures  forces  developed  between  the  end  effector  and  an 
obstruction.  The  joystick  consists  of  a  control  arm  attached 
to  a  three  way  toggle  switch.  Strain  gages  are  also  mounted 
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on  the  joystick  to  detect  forces  applied  to  the  joystick. 
Figure  3.2  provides  a  description  of  the  joystick  and  end 
effector  force  sensors. 


Figure  3.1.  Hydraulic  Force  Override  Rate  Control  System. 
[Ref.  13] 

Movement  of  the  joystick  controls  the  position  of  the 
toggle  switch  which  controls  a  +10/-10  V  voltage  supply  to  the 
servo  valve.  In  the  mid-position  no  voltage  is  supplied  to 
the  servo  valve.  Moving  the  joystick  in  a  forward  direction 
supplies  a  positive  voltage  and  moving  it  in  a  reverse 
direction  supplies  a  negative  voltage.  The  hydraulic 
system  is  arranged  so  that  a  positive  voltage  supply  to  the 
servo  valve  results  in  forward  motion  of  the  end  effector  and 
a  negative  voltage  supply  results  in  reverse  motion  of  the  end 
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Figure  3.2.  Single  OOF  Joystick  and  End  Effector. 

effector.  With  no  forces  acting  on  the  joystick  and  no  forces 
acting  against  the  end  effector  the  piston  moves  at  a  constant 
rate  in  a  direction  based  on  the  voltage  supplied  to  the  servo 
valve. 

Voltages  from  the  strain  gage  amplifiers  of  the  force 
sensors  on  the  joystick  and  the  end  effector  are  added  to  the 
original  +10/-10  V  supplied  by  the  positioning  of  the  joystick 
to  provide  force  override  control  of  the  initial  rate  control. 
When  the  end  effector  comes  into  contact  with  an  obstruction, 
a  voltage  opposing  the  control  voltage  is  developed  by  the  end 
effector  force  sensor.  This  is  summed  with  the  existing 
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controlling  voltage  to  the  servo  valve  and  reduces  the  control 
voltage.  This  reduces  the  speed  of  the  end  effector's  motion. 
This  continues  until  the  control  voltage  to  the  servo  valve 
returns  to  a  zero  value  and  motion  stops.  An  additional  force 
applied  to  the  joystick  will  develop  a  voltage  in  the  force 
sensor  attached  to  the  joystick.  When  added  to  the  existing 
control  voltage  supplied  to  the  servo  valve,  this  additional 
voltage  will  cause  a  non-zero  voltage  to  the  servo  valve  and 
continued  motion  of  the  end  effector.  This  motion  continues 
until  the  feedback  voltage  from  the  force  sensor  on  the  end 
effector  counteracts  the  voltage  due  to  the  applied  force  on 
the  joystick. 

2.  Analysis  and  Testing 

The  hydraulic  system  acts  as  a  valve  controlled  piston 
whose  dynamics  are  developed  in  [Ref.  14].  Figure 

3.3  provides  a  simple  block  diagram  of  the  system  described 
above.  The  following  notation  is  used: 

Ed  :  The  initial  voltage  threshold  supplied  to  the  servo 

valve  when  the  joystick  is  positioned  in  the  forward 
or  reverse  direction.  This  may  be  adjusted  by  the 
operator  and  essentially  establishes  the  initial 
rate  of  piston  motion  with  no  environmental 
constraint. 

Fa  :  The  applied  force  on  the  joystick. 

F  :  The  actual  force  being  applied  by  the  end  effector 

on  the  environment. 

Ef  :  The  sum  of  the  threshold  voltage  and  voltage  due  to 

force  applied  on  the  joystick. 
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Figure  3.3.  Block  Diagram  of  Hydraulic  Control  System. 


ev  :  The  voltage  error  developed  from  force  sensors  and 

the  threshold  voltage  which  is  applied  to  the  servo 
valve . 

xe  :  The  position  of  the  end  effector  with  respect  to  an 

some  stationary  reference  frame. 

x,,  :  The  position  of  the  mass-spring  providing  an 

environmental  constraint  with  the  respect  to  the 
stationary  reference  frame. 

xv  :  The  position  of  the  servo  valve  relative  to  the  null 

position. 

Ka  :  A  proportionality  constant  relating  the  servo  valve 

position  to  the  force  applied  on  the  hydraulic 
piston  by  the  hydraulic  fluid. 

wh  :  The  natural  frequency  of  the  hydraulic  piston  and 

load  arrangement. 

6h  :  The  damping  ratio  of  the  hydraulic  piston  and  load 

arrangement . 
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KE  :  The  overall  stiffness  of  the  end  effector  and 

spring-mass  assembly. 

K$  :  A  proportionality  constant  indicating  the 

relationship  between  the  voltage  applied  to  the 
servo  valve  and  the  position  of  the  servo  valve. 

Ke  :  The  strain  gage  amplifier  gain  of  the  end  effector 

force  sensor. 

Kj  :  The  strain  gage  amplifier  gain  of  the  joystick  force 

sensor. 

The  diagram  assumes  that  the  dynamics  of  the  servo  valve  are 
sufficiently  fast  to  be  negligible  and  the  servo  valve's 
position  is  simply  proportional  to  the  input  voltage.  Also, 
the  disturbance  to  the  piston  dynamics  due  to  the  reaction 
force  at  the  end  effector  is  neglected.  The  closed  loop 
transfer  function  of  the  system  is: 

F(s)  . _ _  (3#1) 

E*{S) 

This  is  a  third  order  system  in  which  the  value  of  KG  can  be 
selected  to  establish  the  desired  sensitivity  of  the  end 
effector  to  force  interactions.  The  value  of  Kj  is  selected 
to  match  the  value  of  KG  so  the  added  force  applied  at  the  end 
effector  matches  the  added  force  applied  at  the  joystick. 
This  system  is  essentially  the  same  as  the  single  DOF  system 
model  developed  in  Chapter  II.  The  dynamics  of  the  system  are 
modelled  slightly  differently  and  the  initial  threshold 
voltage  is  not  present  in  the  theoretical  model.  Qualitative 
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tests  confirm  that  the  system  response  matches  the  expected 
response  of  this  analysis. 

B.  SINGLE  DOF  FORCE  CONTROL  OF  A  PUMA  MANIPULATOR 
1 .  Description 

The  control  structure  of  the  hydraulic  system  is 
applied  to  the  PUMA  560  to  provide  force  override  of  rate 
control  in  a  single  direction.  Figure  3.4  provides  a  general 
diagram  of  the  system. [Ref.  15]  The  joystick 
assembly  and  force  sensor  on  the  end  effector  from  the 
hydraulic  system  are  used  in  controlling  the  PUMA  560.  For 
this  system  however,  an  analog  to  digital  (A/D)  conversion 
circuit  is  required  to  enable  data  from  the  force  sensors  of 
the  joystick  and  end  effector  to  be  communicated  to  the  PUMA 
560 's  control  computer.  The  A/D  converter  is  used  to  convert 
the  error  signal  developed  between  the  joystick  and  the  end 
effector,  not  the  actual  signals  developed  by  each  device. 
This  same  conversion  circuit  is  also  used  in  the  three  DOF 
force  override  rate  controller  and  is  described  in  more  detail 
in  the  next  chapter. 

In  this  system  the  PUMA  560  and  it's  control  computer 
replaces  the  hydraulic  cylinder  and  electro-hydraulic  servo 
valve.  The  direction  of  control  is  along  the  z  axis  of  the 
tool  frame  of  the  manipulator. 
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Figure  3.4.  One  DOF  Force  Override  Rate  Control  of  a  PUMA 
560  Manipulator. [Ref .  15] 

2.  Operation  and  Testing 

Implementing  this  control  method  for  the  PUMA  requires 
developing  the  necessary  software  to  obtain  information  from 
the  force  sensing  circuit  and  to  control  the  motion  of  the 
manipulator  based  on  this  input.  Driels  develops  the  control 
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algorithm  for  this  system  using  the  PUMA  560's  VAL 
programming  language. [Ref .  16]  A  demonstration  of 
the  system  is  performed  which  verifies  the  feasibility  of  the 
control  system.  Qualitative  results  indicate  that  the  system 
performs  as  expected. 

This  leads  to  the  development  and  testing  of  a  control 
system  for  the  PUMA  560  manipulator  that  provides  force 
override  rate  control  in  three  DOF.  The  remaining  chapters 
are  devoted  to  this  endeavor. 
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IV.  THREE  DOF  FORCE  OVERRIDE  RATE  CONTROLLER  FOR  A  PUMA  560 


A.  DESCRIPTION 

1.  Overall  System 

The  control  system  for  force  override  rate  control  of 
the  PUMA  560  manipulator  consists  of  the  original  PUMA  560 
control  system  with  the  force  control  system  built  around  it. 
Figure  4.1  provides  an  overall  description  of  the  system.  The 
PUMA  560 's  control  system  consists  essentially  of  a  PID 
controller  designed  to  control  the  position  and  velocity  of 
the  tool  tip. 

The  force  control  system  added  to  the  PUMA  control 
system  consists  of  an  end  effector  and  joystick  with  strain 
gages  mounted  on  each  to  sense  forces  in  each  direction,  the 
associated  strain  gage  amplifiers,  a  summing  and  biasing 
circuit,  an  A/D  converter,  and  a  control  program  written  in 
VAL.  Each  item  is  discussed  in  some  detail  below. 

2.  End  Effector  and  Joystick 

Rather  than  purchase  sophisticated  and  expensive 
commercially  available  force  sensors  for  the  system, 
relatively  simple  sensors  consisting  of  strain  gages  mounted 
on  an  L-shaped  beam  are  fabricated  to  test  the  feasibility  of 
the  control  system.  Figure  4.2  provides  a  drawing  of  the  end 
effector  and  joystick.  Each  is  made  of  aluminum  and  the 
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Figure  4.1.  Three  DOF  Force  Override  Rate  Controller. 

dimensions  are  chosen  so  as  to  provide  relatively  low 
stiffness  for  good  stability  of  the  force  control  system. 

Strain  gages  are  mounted  as  shown  in  Figure  4.2  to 
provide  the  ability  to  determine  the  forces  applied  by  the  end 
effector  tip  in  each  of  the  tool  frame  cartesian  directions. 

A  calibration  is  performed  to  establish  the  desired 
voltage  output  from  the  strain  gage  amplifiers  for  an  applied 
force.  The  calibration  is  conducted  by  placing  or  hanging 
weights  on  the  end  effector  and  joystick  and  adjusting  the 
amplifier  gain  to  establish  the  desired  output.  Heights  of 
.25  lb  (1.1  N),  0.5  lb  (2.2  N) f  and  1.0  lb  (4.4  N)  nominally 
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are  used  for  the  calibration.  Tables  4.1  and  4.2  provide  the 
calibration  results. 


It  is  important  to  match  the  outputs  of  the  end 
effector  and  joystick  during  the  calibration  since  error 
signals  between  the  two  sensors  are  used  in  the  control 
algorithm,  not  the  actual  signals  from  each  sensor.  This 
arrangement  prevents  compensation  for  calibration  errors  in 
the  software  but  also  reduces  the  amount  of  digital 
computation  and  data  exchange  required. 

Strain  gage  amplifiers  not  specifically  designed  for 
this  system  are  used  due  to  their  immediate  availability.  The 
amplifiers  have  a  limited  gain  adjustment  and  are  generally 
more  sensitive  than  desired  for  this  application.  However, 
their  output  is  of  very  high  quality,  having  very  little 
noise.  The  sensitivity  of  the  amplifiers  and  the  relatively 
low  mechanical  strength  of  the  end  effector  and  the  joystick 
limit  the  magnitude  of  forces  that  can  be  developed  and 
analyzed. 

From  Tables  4 . 1  and  4 . 2  one  can  see  that  there  is 
considerable  coupling  between  strain  gage  3  and  strain  gage  2 
for  forces  applied  in  the  y  direction.  This  creates  problems 
in  utilizing  these  sensors  in  the  control  algorithm.  The 
amount  of  coupling  that  occurs  is  not  the  same  for  the  end 
effector  as  it  is  for  the  joystick.  A  nominal  force  of  2  N 
applied  in  the  y  direction  results  in  a  voltage  of  6.7  V  in 
gage  3  of  the  joystick  and  only  6.0  V  in  gage  3  on  the  end 
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TABLE  4.1.  JOYSTICK  CALIBRATION  DATA 


Direction 

of 

Force 

Strain 

Gage 

No. 

Output  Voltage 
(Volts) 

Applied  Load 

4.4  N 

2.2  N 

1.1  N 

+  X 

1 

8.0 

4.1 

to 

• 

o 

2 

0 

0 

0 

3 

0.40 

0.23 

0.12 

+  y 

1 

-0.10 

-0.05 

CM 

O 

• 

o 

1 

2 

12.0 

6.0 

2.94 

3 

<  -13.0 

-6.7 

-3.4 

-  z 

1 

0.40 

0.13 

0.08 

2 

0 

0 

0 

3 

-3.0 

-1.5 

-0.80 

effector.  These  voltage  outputs  assume  that  gage  3  for  both, 
the  joystick  and  end  effector,  provides  a  3  V  output  for  a  2 
N  force  applied  in  the  z  direction.  This  disparity  cannot  be 
resolved  but  this  does  not  severely  effect  the  testing  of  the 
system. 

The  stiffness  of  the  joystick  and  end  effector  are 
estimated  using  solid  mechanics  principles  of  beam  deflection 
and  superposition.  Approximations  are  made  to  limit  the 
complexity  of  the  analysis.  Transverse  loading  effects  of 
forces  applied  on  the  sensors  are  taken  into  account  and  pure 
bending  is  assumed.  Torsional  loading  of  lower  beam,  however, 
is  taken  into  account  when  evaluating  stiffness  in  the  x 
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TABLE  4.2.  END  EFFECTOR  CALIBRATION  DATA 


Direction 

of 

Force 

Strain 

Gage 

No. 

Output  Voltage 
(Volts) 

Applied  Load 

4.4  N 

2.2  N 

1.1  N 

+  X 

1 

-8.0 

-4.2 

-2.0 

2 

0 

0 

0 

3 

■■■ 

■m 

mi 

+  y 

1 

2 

-12.0 

-6.0 

-2.95 

3 

>  +12.0 

+6.2 

+3.0 

-  z 

1 

■■■1 

■ ■* 

2 

I— ill 

■ 

:  : 

j 

3 

+3.0 

-+.5 

o 

00 

• 

o 

+ 

direction.  Results  of  the  analysis  for  stiffness  are  listed 
in  Table  4.3.  The  results  indicate  that  the  stiffness  in  the 
y  direction  is  approximately  one  half  the  stiffness  in  the  x 
direction  and  the  stiffness  in  the  z  direction  is  very  high 
compared  to  the  stiffness  in  the  x  and  y  directions.  This 
leads  to  control  and  stability  problems  which  are  discussed  in 
the  next  chapter. 

The  strain  gage  amplifiers  utilize  a  two  gage  bridge 
and  develop  an  output  in  the  range  of  -12/+12  V.  The 
amplified  signal  is  sent  to  a  summing  and  biasing  circuit  as 
well  as  a  data  acquisition  board  used  for  recording  force 
data. 
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TABLE  4.3.  END  EFFECTOR  AND  JOYSTICK  ESTIMATED  STIFFNESS 


Direction 

Stiffness 

(N/mm) 

X 

1.205 

y 

0.525 

z 

104.3 

3.  Summing  and  Biasing  Circuit 

Figure  4.3  is  a  diagram  of  the  summing  and  biasing 
circuit.  This  circuit  produces  a  voltage  signal  based  on  the 
difference  in  the  forces  developed  at  the  joystick  and  those 
developed  at  the  end  effector.  The  output  of  each  strain  gage 
on  the  joystick  is  summed  with  the  output  of  the 
corresponding  strain  gage  on  the  end  effector.  The  strain 
gages  of  the  end  effector  are  connected  in  such  a  way  that 
output  voltages  of  its  gages  are  opposite  in  sign  from  those 
of  the  joystick  for  a  given  applied  force.  This  results  in  a 
voltage  signal  in  the  summing  circuit  proportional  to  the 
force  error  between  the  joystick  and  the  end  effector  in  the 
range  of  +12/-12  V.  This  signal  is  then  converted  to  a  range 
of  0  to  +5  V  in  an  analog  biasing  and  attenuation  circuit. 
This  step  is  required  for  proper  operation  of  the  A/D 
converter  which  converts  an  analog  voltage  signal  in  this 
range . 

4 .  A/D  Converter 

The  A/D  converter  is  a  National  Semiconductor  ADC816 
converter  which  allows  for  16  channels  of  analog  data 
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Figure  4.3.  Summing  and  Biasing  Circuit. 

conversion  to  digital  signals. [Ref .  17]  The  device 
uses  power  supplied  by  the  PUMA  controller  and  converts  a  0  to 
+5V  analog  voltage  input  into  an  eight  bit  digital  value.  The 
converter  is  operated  in  free-run  mode  with  an  internal 
oscillator  allowing  200  conversions/sec. [Ref .  18] 
The  converter  utilizes  a  multiplexer  to  select  the  channel  to 
converted.  Of  the  16  available  channels  only  four  are  wired 
for  this  application  and  only  three  of  these  channels  are 
actually  used.  The  output  is  wired  to  one  of  the  four  eight 
bit  digital  input/output  (I/O)  ports  of  the  PUMA  560. 
controller.  A  diagram  of  the  A/D  converter  is  shown  in  Figure 
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4.4. [Ref.  19]  Figure  4.5  provides  a  wiring  diagram 
of  the  data  conversion  circuit  cable  interface  with  the  PUMA 
560  I/O  port. [Ref .  20] 

5.  Computer  Input/Output  Interface 

The  PUMA  560  has  the  capability  of  transferring  32 
bits  of  digital  data  using  four  I/O  ports  of  one  byte  each. 
The  bits  are  numbered  1-32,  with  1-8  assigned  to  I/O  port  1, 
9-16  assigned  to  I/O  port  2,  17-24  assigned  to  port  3,  and  25- 
32  to  port  4.  The  SIGNAL  command  in  the  VAL  language  is  used 
for  output  purposes  while  the  BITS  command  is  used  for  input. 
A  sample  statement  using  the  SIGNAL  command  is  as  follows: 

SIGNAL  1, -2, 3, -4 

The  numbers  correspond  to  the  bit  number  defined  above  and  the 
sign  indicates  the  voltage  state  of  that  bit.  A  minus(-)  sign 
turns  the  bit  off  (a  high  voltage)  while  the  plus(+)  sign  or 
no  sign  turns  the  bit  on  (low  voltage).  This  command  controls 
which  channel  of  data  the  A/D  converter  uses  as  output  to  the 
PUMA  control  program. 

The  input  from  the  A/D  converter  is  obtained  using  the 
BITS  command  as  in  the  following  example: 

BITS (1003 ,8) 

The  first  number  corresponds  to  the  address  of  the  first  data 
bit  to  be  received,  where  1001  corresponds  to  data  bit  1,  1002 
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to  data  bit  2,  and  up  to  1032  corresponding  to  bit  32.  The 
second  number  indicates  the  number  of  data  bits  to  be 
received.  In  this  example  statement  the  values  of  data  bits 
3  through  11  would  be  received. 
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Figure  4.5.  A/D  Converter  Interface  Cable  Hiring  Diagram. 
[Ref.  20] 


The  Unimat ion  Corporation  does  market  an  analog  I/O 
board  for  use  with  the  PUMA  560  but  the  equipment  described 
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above  was  on  hand  and  is  utilize  to  save  on  expenses  and 
provides  more  flexibility. 


€ .  Environment 

Four  types  of  environmental  constraints  are  used  to 
evaluate  the  performance  of  the  system.  The  same  spring  mass 
assembly  used  in  the  preliminary  work  of  the  hydraulic  control 
system  and  in  demonstrating  the  single  DOF  system  of  the  PUMA 
manipulator  are  used  to  simulate  a  compliant  stationary 
constraint.  The  aluminum  block  support  for  the  spring  mass 
assembly  is  placed  in  the  manipulator's  work  space  to  provide 
a  rigid  stationary  constraint.  The  joystick  used  in  the 
preliminary  work  is  also  placed  in  the  work  space  to  provide 
a  task  oriented  constraint  with  which  to  experiment  with. 
Finally,  a  small  mass  placed  on  a  low  friction  surface  in  the 
work  space  allows  for  testing  of  the  manipulator  in  handling 
a  simple  inertial  load. 

7.  Force  Data  Acquisition  and  Recording 

The  outputs  of  the  strain  gage  amplifiers  are  wired 
into  a  +10/-10  V  A/D  data  acquisition  board  which  is  connected 
to  an  IBM  XT  computer.  This  data  acquisition  board  supports 
up  to  16  channels  of  analog  data.  Six  channels  are  utilized 
to  record  the  outputs  from  the  strain  gage  amplifiers  into  a 
data  file.  The  processing  speed  of  the  computer  allows  for  a 
data  acquisition  rate  of  approximately  85  Hertz  for  recording 
data.  This  is  fast  enough  to  obtain  accurate  force  data  from 
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the  system  since  the  system  dynamics  occur  at  a  considerably 
lower  rate.  An  effort  is  made  to  use  the  data  acquisition 
system  to  perform  some  type  of  force  display  but  limitations 
in  the  computer's  processing  speed,  the  monitor's 
capabilities,  and  the  communication  time  required  between 
these  two  devices  prevent  this . 

8.  PUMA  560  Control  System 

The  PUMA  560  control  system  has  a  main  computer 
processor  which  supplies  motion  commands  to  individual 
microprocessors  which  control  each  joint  servo  motor.  Figure 
4.6  provides  a  diagram  of  the  system  structure. 

Each  joint  is  controlled  as  a  basic  servomechanism 
with  a  Rockwell  6503  microprocessor  which  uses  a  PID  control 
law  to  position  the  joint. [Ref.  21]  The  integral 
control  can  be  deactivated  with  the  INTOFF  VAL  command.  Each 
joint  follows  the  dynamics  developed  for  joint  space  control 
in  Chapter  II.  This  inner  loop  operates  with  a  0.875  msec 
sample  time  and  each  joint  controller  receives  an  updated 
position  command  from  the  main  controller  every  28  msec. 

The  main  computer  is  a  DEC  LSI  11/02  processor. 
[Ref.  22]  It  performs  the  primary  functions  of 
processing  user  motion  commands  entered  in  the  VAL  programming 
language  and  coordinating  the  actions  of  the  six  joint  servo 
controllers.  The  main  computer  performs  all  coordinate 
transformations  between  joint  space  and  cartesian  space 
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Figure  4.6.  PUMA  560  Control  System  Diagram. 

positions  and  develops  incremental  position  commands  to  the 
individual  joint  servos  every  28  msec. [Ref.  23] 
These  position  commands  are  based  on  motion  commands  entered 
through  user  programs.  The  processor  looks  ahead  to  the  next 
motion  command  in  order  to  coordinate  continuous  path  motion. 
[Ref.  24]  The  motion  of  the  manipulator  remains 
continuous  by  adjusting  from  one  commanded  position  to  the 
next.  Considerable  computational  time  is  required  for  this 
and  if  a  particular  motion  does  not  take  enough  time  the 
manipulator  will  not  be  able  to  develop  a  continuous  path 
trajectory  for  the  next  motion  command.  This  results  in  jerky 
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motion  as  the  manipulator  stops  momentarily  between  individual 
positions.  The  continuous  path  trajectory  planning  can  be 
deactivated  to  stop  the  robot  after  completing  each  motion 
command. 

The  computer  also  has  the  capability  to  perform 
straight  line  motion  or  joint  interpolated  motion  as  discussed 
in  chapter  two.  Considerably  more  computations  are  required 
to  handle  the  transformations  involved  with  straight  line 
motion.  Each  commanded  motion  must  take  approximately  140 
msec  in  order  to  maintain  continuous  path  motion  while  about 
60  msec  is  needed  while  using  joint  interpolated  motion. 
[Ref.  25]  For  this  reason  joint  interpolated  motion  is  used 
in  this  application  where  small  position  changes  are  used  and 
the  difference  between  a  straight  line  trajectory  and  joint 
interpolated  trajectory  is  minimal. 

9 .  Control  Program 

Appendix  A  contains  a  copy  of  the  VAL  program  written 
to  implement  the  three  DOF  force  override  rate  control 
algorithm  with  the  PUMA  560  manipulator.  It  consists  of  three 
general  sections.  The  initial  section  has  the  user  enter 
values  for  the  various  control  gains  and  provides  some  option 
as  to  the  structure  of  the  control  system.  The  next  section 
initialize  the  force  error  values  from  force  error  circuits. 
The  final  section  is  a  continuous  loop  which  provides  position 
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and  rate  commands  to  the  main  controller.  Figure  4.7  provides 
a  flow  diagram  of  the  control  program. 

The  operator  initializes  the  system  control  parameters 
which  includes  specifying  the  value  of  3,  as  defined  in 
Chapter  II,  to  be  used  for  each  direction  and  whether  or  not 
integral  control  is  to  be  used  within  the  joint  servo  control 
loops.  The  NONULL  ALWAYS  command  of  step  22  deactivates  the 
normal  processing  sequence  in  which  the  main  processor  waits 
for  reports  from  each  servo  processor  that  the  current 
incremental  position  has  been  reached  before  a  new  position  is 
ordered.  This  reduces  delays  in  the  program  execution  to 
allow  a  faster  response  and  less  time  lag  in  processing  motion 
commands . 

All  of  the  adjustable  control  system  gains  are  also 
defined  in  this  first  section.  The  value  of  adf actor  is  a 
conversion  of  the  values  received  at  the  PUMA'S  I/O  port  to 
the  voltage  outputs  from  the  summing  circuit.  The  values  of 
xamp.gain,  y amp. gain,  and  zamp.gain  provide  conversions  from 
the  voltages  out  of  the  summing  circuit  to  the  actual  force 
errors  between  the  end  effector  and  joystick.  Steps  32 
through  34  set  the  values  of  xforce.gain,  y force. gain,  and 
zforce.gain  which  correlate  to  the  term  KFP  in  the  block 
diagram  of  Figure  2.7.  Step  44  specifies  a  value  for 
movetime,  an  estimate  of  the  required  of  each  motion  command 
to  maintain  continuous  path  motion.  Step  45  defines  maxspeed 
as  an  estimate  of  the  maximum  linear  velocity  the  PUMA  560  can 
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Figure  4.7.  Control  Program  Flow  Diagram. 
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move  in  mm/sec.  This  value  is  based  on  tabulated  PUMA 
operating  parameters  ([Ref.  26])  and  is  needed  in 
step  98  to  provide  a  basis  for  specifying  a  speed  for  the  next 
motion . 

The  second  section  goes  through  a  series  of  steps 
which  obtain  initial  values  from  the  A/D  converter 
corresponding  to  force  error  signals.  This  section  begins 
with  step  52  and  continues  through  step  68.  These  values  are 
assumed  to  be  the  force  error  signals  when  no  forces  are 
acting  on  the  end  effector  or  the  joystick.  If  the  system  is 
adjusted  perfectly  with  the  zero  adjustment  on  the  strain  gage 
amplifiers  set  just  right  the  values  of  xcal,  ycal  and  zeal 
would  be  128.  This  section  provides  a  means  to  effectively 
zero  the  force  errors  without  having  to  physically  change  the 
zero  adjustments  on  the  strain  gage  amplifiers.  This  does  not 
prevent  one  from  needing  to  manually  calibrate  the  gain 
adjustments  on  the  strain  gage  circuits  as  discussed  before. 
The  sequence  of  steps  is  explained  below. 

Step  52  is  a  SIGNAL  command  which  sends  a  signal  from 
the  PUMA  controller  through  I/O  port  1  to  the  A/D  converter 
specifying  the  A/D  converter  to  convert  channel  0  to  a  digital 
value  for  output  to  the  PUMA'S  I/O  port.  A  short  FOR  -  NEXT 
loop  provides  a  delay  before  a  BITS  input  command  is  used  to 
assign  the  value  of  the  current  channel  selected  for  output  in 
the  A/D  converter  to  xcal.  The  delay  is  experimentally 
determined  to  be  necessary  to  allow  the  A/D  converter's 
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multiplexer  enough  time  to  switch  to  the  desired  channel 
before  its  output  is  obtained  using  the  BITS  command.  This 
sequence  of  steps  is  repeated  twice  to  obtain  the  values  of 
ycal  and  zeal  from  channel  1  and  2  of  the  A/D  converter, 
respectively. 

The  final  section  starts  at  line  72  which  initiates 
the  force  error  input  sequence.  The  values  for  xin,  yin, 
and  zin  are  assigned  values  using  the  same  sequence  of  steps 
used  to  obtain  the  values  of  xcal,  ycal,  and  zeal  which  are 
the  current  values  of  the  force  error  signals  at  the  start  of 
each  control  loop.  In  steps  87  through  92  the  xin,  yin,  and 
zin  are  subtracted  from  the  initial  calibration  values  of 
xcal,  ycal,  and  zeal,  respectively  and  the  values  are 
decoupled  to  obtain  force  errors  in  the  three  cartesian 
directions  defined  by  the  manipulators  tool  frame.  In  steps 
93  through  95  the  force  errors  are  multiplied  by  a  force  gain 
and  the  integral  of  the  force  error  is  multiplied  by  a 
corresponding  gain  (if  integral  control  of  forces  is  used)  to 
obtain  the  distance  the  manipulator  is  to  move  in  each 
direction.  These  values  are  assigned  to  xmove,  ymove,  and 
zmove.  The  overall  distance  is  calculated  in  step  96  and  a 
speed  is  calculated  in  step  97  so  that  the  motion  will  take 
the  required  time  to  allow  continuous  path  motion.  An  actual 
linear  speed  cannot  be  specified  with  joint  interpolated 
motion  but  the  speed  must  be  defined  relative  to  the  normal 
speed  defined  internally  within  the  VAL  controller.  This 
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relative  speed  is  calculated  in  step  98  which  provides  the 
commanded  speed  for  the  next  motion  command.  The  normal  speed 
of  each  manipulator  joint  is  defined  in  the  PUMA 
manual. [Ref.  27]  Step  99  defines  a  coordinate 
transformation  based  on  the  distance  the  manipulator  needs  to 
move  in  each  direction  and  assigns  it  to  delta.  Step  100 
assigns  a  coordinate  transformation  defined  by  the  current 
manipulator  position  to  initial  and  step  101  is  a  joint 
interpolated  motion  command  for  the  manipulator  to  move  to  the 
location  defined  by  adding  the  delta  transformation  to  the 
initial  position.  Steps  102  through  104  numerically  integrate 
the  force  error  in  each  direction  using  a  simple  Euler  method 
and  assign  the  values  to  inxferr,  intyferr,  and  intzferr. 

B.  EXPERIMENTAL  TESTS 

1.  Force  Control  of  Stationary  Constraint 
a.  Description 

Several  tests  are  performed  testing  the  response  of 
the  system  when  interacting  with  the  compliant  and  rigid 
stationary  constraints  described  earlier.  Tests  are  performed 
to  evaluate  the  manipulator's  response  in  each  of  the  three 
cartesian  directions.  The  manipulator  is  aligned  a  nominal 
distance  of  5  mm  away  from  the  constraint  and  a  step  force 
input  is  applied  to  the  joystick.  This  force  input  is  applied 
by  hanging  the  same  weights  used  to  calibrate  the  joystick  on 
the  end  of  the  joystick.  This  provides  a  relatively  steady 
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force  input  without  having  to  substitute  a  test  signal  for  the 
joystick  strain  gage  inputs.  The  joystick  is  oriented  in  such 
a  way  that  the  gravitational  loading  of  the  weight  is  applied 
in  the  desired  direction.  The  constraint  is  also  positioned 
to  ensure  that  the  constraint  is  perpendicular  to  the 
direction  of  the  commanded  force  and  manipulator  motion.  When 
testing  the  response  in  the  z  direction  the  compliant  spring- 
mass  system  is  utilized  to  ensure  adequate  compliance  in  an 
otherwise  extremely  stiff  environment  and  the  manipulator  is 
positioned  at  a  point  just  prior  to  contact  with  the 
constraint. 

System  response  is  obtained  for  step  force  inputs 
of  different  magnitudes  and  with  different  values  of  (3  used  in 
the  control  program.  The  response  is  obtained  for  tests  in 
which  integral  control  of  the  individual  joint  position  servo 
controllers  is  activated  and  when  it  is  de-activated.  Tests 
are  also  run  to  determine  the  response  of  the  system  when 
integral  control  of  the  force  error  signal  is  used.  During 
these  tests  the  manipulator  is  positioned  so  as  to  just  be 
making  contact  with  the  stationary  constraint. 

Tests  are  also  run  to  check  the  system  response  to 
a  relatively  low  frequency  sinusoidal  force  input.  The  strain 
gage  amplifier  output  from  the  joystick  in  the  x  direction  is 
replaced  by  a  sinusoidal  voltage  from  a  function  generator. 
The  test  is  run  at  two  frequencies  and  varying  values  of  3  to 
demonstrate  the  variations  in  magnitude  and  phase  lag  of  the 
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force  applied  by  the  end  effector  to  the  rigid  constraint. 
The  coupling  between  the  outputs  of  the  strain  gages  in  the  y 
and  z  directions  make  it  difficult  to  perform  this  test  in 
either  of  these  directions  and  tests  in  these  directions  are 
not  performed. 

b.  Results 

(1)  Step  Input 

Figure  4.8  shows  the  force  response  to  a  0.5  lb 
(2.2  N)  nominal  force  command  in  the  x  direction  with  |3=0.5 
and  using  PID  control  in  the  individual  joint  servos.  The  top 
graph  is  the  response  in  the  x  direction,  the  middle  graph  is 
the  response  in  the  y  direction,  and  the  bottom  graph  is  the 
response  in  the  z  direction.  Figures  4.9,  4.10,  and  4.11  show 
the  response  for  13=1,  (3=2,  and  13=4  with  PID  control  used  in 
the  individual  joint  servo  position  controllers.  Figures 
4.12,  4.13,  and  4.14  show  the  results  for  a  force  input  in  the 
y  direction  while  using  the  same  system  control  parameters. 
Figure  4.15  shows  the  response  of  the  system  to  a  force  input 
of  4.0  N  in  the  z  direction  with  |3=20,  (3=40,  and  3=60  in  the 
z  direction,  respectively,  and  3=1  in  the  x  and  y  directions. 
The  top  graph  is  of  3=20,  the  middle  graph  is  for  3=40,  and 
the  bottom  graph  is  for  3=60.  Using  values  of  3  much  lower 
than  these  in  the  z  direction  results  in  a  very  slow  response 
due  to  the  high  end  effector  stiffness  in  this  direction. 


57 


Force  (N)  Force  ^ 


Time  (sec) 


Figure  4.8.  Step  Force  Input  in  the  x  Direction ,  &*0.5. 
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Figure  4.9.  Step  Force  Input  in  the  x  Direction,  3=1 . 
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Figure  4.10.  Step  Force  Input  in  the  x  Direction,  0=2 
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Figure  4.11.  Step  Force  Input  in  the  x  Direction,  &=4 . 
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Figure  4.13.  Step  Force  Input  in  the  y  Direction,  6=2 
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Figure  4.15.  Step  Force  Input  in  the  z  Direction,  B=20,  5=40 


Only  the  response  in  the  z  direction  is  shown  for  the  input 
force  in  that  direction  and  the  compliant  constraint  is 
utilized. 

Figures  4.16  and  4.17  show  the  response  to  a 
4.4  N  nominal  force  command  in  the  x  direction  with  3*1. 
Figure  4.16  uses  PID  control  for  the  individual  joint  servos 
and  Figure  4.17  uses  PD  control  in  the  individual  joint 
servos . 

(2)  Sinusoidal  Input 

Figure  4.18  shows  the  force  response  of  the 
manipulator  when  subject  to  a  sinusoidal  force  command  in  the 
x  direction  at  a  frequency  of  0.33  Hertz  and  with  3=1,  3=2, 
and  3=4,  respectively.  The  top  graph  is  for  3=1,  the  middle 
graph  is  for  3=2  and  the  bottom  graph  is  for  3=4.  Only  the 
response  in  the  x  direction  is  shown  and  integral  control  is 
used  in  the  individual  joint  servo  controllers.  Figure  4.19 
shows  the  response  for  the  same  force  command  and  values  of  3, 
but  with  no  integral  control  in  the  individual  joint  servo 
loops . 

Figure  4.20  shows  the  response  for  a  1.25  Hertz 
force  command  and  3=1  and  3=2,  with  integral  control  in  the 
individual  joint  servo  loops.  The  top  graph  is  for  3=1  and 
the  bottom  graph  is  for  3=2.  Figure  4.21  shows  the  force 
response  in  each  of  the  three  directions  for  a  1.25  Hertz 
force  input  in  the  x  direction  with  3=2  and  no  integral 
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Time  (sec) 


Figure  4.16.  Step  Force  Input  in  the  x  Direction  of  4.4  N, 
P*!,  and  PID  Control  of  Joint  Servos. 
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Figure  4.19.  0.3  Hz  Force  Input ,  B=1 , 2  and  4,  PD  Control  of 

Joint  Servos. 
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Figure  4.20.  1.25  Hz  Force  Input,  3=1  and  2,  PID  Control 

Joint  Servos. 
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Figure  4.21.  1.25  Hz  Force  Input,  3=2 ,  PD  Control  of  Joint 

Servos . 


control  in  the  joint  servo  control  loops.  As  before,  the  top 
graph  is  the  x  direction,  the  middle  graph  is  the  y  direction, 
and  the  bottom  graph  is  the  z  direction. 

2.  Rate  Control  in  Free  Space 

a.  Description 

In  addition  to  the  force  response  above, 
qualitative  data  is  obtained  from  controlling  the  manipulator 
in  free  space.  This  allows  for  an  evaluation  the  speed  of 
response  and  the  effectiveness  of  the  control  algorithm  of 
producing  the  characteristics  of  a  rate  control  system  when  no 
force  constraints  exist  at  the  end  effector.  This  also  tests 
the  ability  to  position  the  end  effector  at  a  desired  location 
using  the  joystick. 

The  joystick  is  used  to  move  the  manipulator  around 
in  free  space  under  rate  control.  A  force  applied  to  the 
joystick  is  varied  in  magnitude  and  direction  and  the  motion 
of  the  manipulator  is  monitored.  The  values  of  maxspeed  and 
movetime  in  the  control  program  are  varied  to  determine  their 
effect  on  the  response. 

b.  Results 

The  manipulator  maintains  a  continuous  path  motion 
for  values  of  movetime  down  to  40  msec  in  several  manipulator 
orientations  tested,  with  a  maxspeed  value  of  462  mm/sec. 
With  a  constant  value  of  movetime  set  of  60  msec,  the  value  of 
maxspeed  is  lowered  form  462  mm/sec  until  continuous  path 
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motion  stops  and  the  manipulator's  motion  becomes  jerky.  The 
value  of  maxspeed  at  which  this  occurs  varies  depending  on  the 
orientation  of  the  manipulator.  Remember  that  maxspeed  is  an 
estimate  of  the  maximum  speed  of  the  manipulator  in  cartesian 
coordinates  which  is  used  as  a  devisor  in  determining  the 
distance  for  incremental  motions  in  the  control  program. 
Continuous  path  motion  is  lost  for  values  of  maxspeed  in  the 
range  of  200-300  mm/sec. 

Using  the  same  value  of  3  in  each  direction 
resulted  in  varying  rates  of  motion  in  each  direction.  The 
value  of  3  must  be  raised  considerably  to  achieve  a  reasonable 
rate  of  response  in  the  z  direction. 

3 .  Task-Oriented  Tests 
a .  Description 

Tests  are  performed  to  demonstrate  the  ability  to 
position  the  end  effector  against  the  single  axis  joystick 
utilized  in  the  preliminary  work  described  in  chapter  two. 
This  single  axis  joystick  serves  as  an  environmental 
constraint.  The  manipulator's  end  effector  is  first 
positioned  against  the  constraint  and  then  a  force  is  applied 
to  attempt  to  reposition  the  constraint.  This  test  is 
particularly  representative  of  the  type  of  tasks  this  system 
would  be  best  suited  for. 
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b.  Results 


The  manipulator  is  positioned  against  the 
constraint  with  little  difficulty  and  force  is  applied  by  the 
manipulator.  The  force  input  is  slowly  increased  until  the 
toggle  switch  resistance  is  overcome  and  the  constraint  is 
repositioned.  Contact  is  lost  between  the  manipulator  and 
constraint  during  this  motion  as  the  constraint's  springing 
action  to  the  next  position  occurs  rapidly.  The  manipulator 
then  moves  until  it  makes  contact  with  the  constraint  again. 
Motion  continues  until  forces  are  built  up  proportional  to  the 
commanded  force  applied  at  the  joystick. 

Once  the  constraint  has  been  repositioned  an 
additional  force  is  then  applied  to  the  constraint.  In  this 
situation  the  manipulator  slides  around  the  spherically  shaped 
constraint  and  the  manipulator  continues  moving  around  the 
constraint.  The  manipulator  actually  slides  around  the 
constraint  as  depicted  in  Figure  4.22. 

4.  Control  of  Inertial  Loads 
a .  Description 

Tests  are  conducted  to  evaluate  the  performance  of 
the  system  when  handling  an  inertial  load  at  the  tip  of  the 
end  effector.  A  small  disk  is  placed  on  a  flat,  low  friction 
surface  in  the  manipulator's  work  space.  The  mass  has  a  hole 
countersunk  into  it  large  enough  to  insert  the  tip  of  the  end 
effector  with  considerable  clearance.  Tests  are  conducted  to 
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Figure  4.22.  Response  with  a  Spherical  Constraint. 

position  the  end  effector  in  this  recess  and  move  the  disk 
around  on  the  flat  surface.  This  approximates  the  response  of 
handling  an  inertial  load  in  two  dimensions. 
b.  Results 

The  end  effector  is  positioned  into  the  hole  of  the 
disk  with  little  difficulty  using  the  joystick  as  a  rate 
control  input  to  the  manipulator.  Once  contact  is  made 
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between  the  end  effector  and  the  disk  a  steady  force/rate 
command  is  applied  to  the  joystick.  The  disk  and  manipulator 
begin  to  move  in  the  commanded  direction  but  oscillate  while 
moving.  The  disk  slows  down  or  stops  moving  momentarily 
during  the  oscillations. 

C.  SYSTEM  SIMULATION 
1 .  Description 

A  BASIC  program  is  written  to  simulate  the  actual 
control  structure  of  the  PUMA  560  in  a  single  DOF.  The 
dynamics  of  the  manipulator  are  assumed  to  be  the  critically 
damped  or  slightly  underdamped  system  used  for  the  system 
analysis  of  chapter  two.  A  copy  of  the  simulation  program  is 
provided  in  Appendix  B.  Figure  4.23  shows  a  flow  diagram  for 
the  program. 

The  simulation  algorithm  consists  of  three  loops.  The 
inner  most  loop  provides  the  simulation  for  the  actual 
dynamics  of  a  joint  servo  as  described  in  the  previous 
section.  A  time  step  of  0.875  msec  is  chosen  since  this  is 
the  update  time  of  the  PUMA  560 's  joint  servo  control  loops. 
The  next  loop  simulates  the  updated  desired  joint  positions 
calculated  every  28  msec  by  the  main  controller  and  commanded 
to  each  joint  servo  controller.  The  outermost  loop  simulates 
the  updated  joint  position  and  velocity  commands  calculated 
and  provided  to  the  main  controller  from  the  VAL  control 
program  as  discussed  earlier  in  this  chapter.  A  brief 
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description  of  each  of  the  variables  used  in  the  program  is 
listed  at  the  beginning  of  the  program. 

A  delay  in  processing  new  position  and  velocity 
commands  is  simulated  within  the  outer  two  loops.  The 
velocity  command  is  based  on  performing  a  motion  command  in 
approximately  140  msec.  This  value  was  experimentally 
determined  by  adjusting  this  value  and  comparing  simulation 
results  to  the  actual  response.  The  program  requires  a 
minimum  of  two  28  msec  updates  from  the  main  controller  to  the 
joint  servo  controllers  for  each  motion  command.  This 
simulates  the  minimum  of  approximately  60  msec  between  motion 
commands  for  continuous  path  motion.  New  force  data,  which 
the  next  motion  command  is  based  on  in  the  control  program,  is 
estimated  to  occur  at  the  end  of  the  first  28  msec  inner  loop 
time  period.  This  new  motion  command  is  not  actually 
performed  until  the  completion  of  the  current  motion  command. 
This  is  designed  to  take  one  to  two  more  28  msec  time  periods. 
This  effectively  inserts  a  28-56  msec  computational  time  delay 
in  the  control  structure. 

2 .  Results 

The  simulation  program  is  run  using  various  values  for 
the  term  3  and  estimates  of  system  stiffness  values  and  other 
control  values.  The  effort  is  made  to  simulate  the  same 
conditions  used  in  the  experiments  of  the  actual  system 
described  above. 
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Figure  4.24  shows  the  response  for  simulations  with 
3=0.5,  3=1.0,  and  3=2  for  an  end  effector  stiffness  being  the 
estimated  stiffness  for  the  x  direction  in  actual  system.  The 
commanded  force  is  a  2.2  N  step  input.  Figure  4.25  shows  the 
simulation  responses  with  the  same  conditions  but  with  the 
stiffness  being  the  estimated  stiffness  in  the  y  direction. 

Figure  4.26  is  the  simulation  response  to  a  0.3  Hz 
force  input  with  3=0.5,  3=1.0,  and  3=2  and  an  end  effector 
stiffness  being  the  estimated  stiffness  for  the  x  direction  in 
actual  system.  Figure  4.27  shows  the  simulations  to  a  1.25  Hz 
force  input  with  the  same  system  parameters.  Figure  4.28 
shows  the  simulations  to  a  1.25  Hertz  force  input  with  the 
same  system  parameters  but  with  3=0.25. 
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Figure  4.25.  Simulation  Response  to  a  Step  Input  in  the  y 
Direction,  B=0.5,  1,  and  2. 
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Figure  4.26.  Simulation  Response  to  a  0.3  Hs  Input  in  the 
Direction ,  0=0.5,  1/  and  2. 
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Figure  4.27.  Simulation  Response  to  a  1.25  Hz  Input,  3=0 . 5 
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Figure  4.28.  Simulation  Response  to  a  1.25  Hz  Force  Input, 
8=0.25. 
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V. 


DISCUSSION 


A.  SYSTEM  PERFORMANCE 

1.  Motion  in  Free  Space 

The  manipulator  behaves  as  expected  during  rate 
control  tests  in  free  space.  The  control  program  parameters 
of  movetime  and  maxspeed  can  be  adjusted  to  provide  a  quick 
response  while  maintaining  continuous  path  motion.  Without 
this  continuous  path  motion  capability  in  the  PUMA  control 
system,  the  system  response  is  relatively  poor.  The 
capabilities  of  the  control  system  of  the  manipulator  greatly 
influence  the  ability  to  apply  this  force  override  rate 
control  system  to  it.  Other  systems  without  continuous  path 
control  would  not  perform  well  with  this  control  algorithm. 

Using  the  same  value  of  3  in  each  direction  results  in 
a  varying  rate  of  response  in  each  direction  for  the  same 
deflection  of  the  joystick.  This  is  due  to  the  differing 
stiffness  values  for  the  end  effector  and  joystick  in  each 
direction.  Difficult  control  of  the  manipulator  in  some 
situations  results,  especially  when  trying  to  move  the  end 
effector  in  the  z  direction.  The  high  stiffness  of  the  end 
effector  in  this  direction,  combined  with  the  coupling  between 
the  strain  gages  measuring  forces  in  the  y  and  z  directions, 
makes  motion  control  in  the  z  direction  very  difficult.  Using 


86 


an  end  effector  and  joystick  with  the  same  stiffness  in  all 
directions  would  improve  controllability  in  this  respect. 
Additionally,  reducing  the  amount  of  coupling  between  force 
sensors  in  each  direction  would  also  improve  performance. 
These  two  issues  must  be  considered  when  designing  and 
constructing  a  new  end  effector  and  joystick  for  the  system. 

As  mentioned  in  the  last  chapter,  the  rate  of  the 
manipulator's  motion  depends  on  the  orientation  of  the  robot 
during  the  motion  and  the  direction  of  the  motion.  The  rate 
commands  in  the  control  program  provide  a  relative  speed  of 
motion  in  joint  space,  not  cartesian  space.  This  change  in 
the  rate  of  motion  is  clearly  noticeable  during  motion  control 
but  does  not  adversely  effect  the  ability  to  position  the 
manipulator.  An  updated  version  of  the  VAL  programming 
language  allows  for  control  of  the  speed  in  cartesian  space 
when  performing  straight  line  motion  commands.  This  would 
alleviate  this  problem  of  varying  speed  but  the  delays 
associated  with  the  additional  computations  required  for  the 
straight  line  motion  would  adversely  effect  the  stability  of 
the  system. 

Efforts  to  control  the  system  with  integral  control  of 
the  force  error  signals  does  result  in  motion  that  is  not 
characteristic  of  rate  control  but  more  like  acceleration 
control.  Difficulty  in  stopping  the  manipulator  at  a  desired 
location,  or  stopping  it  at  all,  becomes  quite  difficult  and 
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renders  this  type  of  control  unacceptable  for  motion  control 
in  free  space. 

2.  Response  with  Stationary  Constraint 
a.  Step  Force  Input 

The  response  is  essentially  that  of  a  critically 
damped  second  order  system.  Although  the  force  response  does 
not  indicate  the  manipulator's  motion  directly,  the 
manipulator  initially  accelerates  to  a  constant  speed 
proportional  to  the  force  error  between  the  joystick  and  end 
effector.  Noticeable  force  oscillations  occur  in  each 
direction  as  the  manipulator  moves.  This  is  shown  in  Figures 
4.8  through  4.12,  4.18,  and  4.19.  The  frequency  of  these 
oscillations  decrease  considerably  after  contact  is  made  with 
the  constraint.  This  oscillation  is  due  to  the  vibration  of 
the  end  effector  as  the  manipulator  moves.  Once  contact  is 
made  with  the  constraint  the  end  effector  acts  as  a  cantilever 
beam  with  a  very  large  mass  at  the  end.  The  natural  frequency 
of  the  end  effector  in  contact  with  the  constraint  decreases 
considerably.  Some  fluctuations  occur  in  the  commanded  force 
also,  as  observed  in  Figures  4.9,  4.11,  4.12,  and  4.19.  This 
is  due  to  some  vibration  of  the  joystick  resulting  from 
machinery  operating  near  by. 

Once  contact  is  made,  some  forces  are  developed  in 
the  directions  other  than  the  direction  of  the  commanded 
force.  This  can  be  seen  in  Figures  4.8  through  4.14.  For 
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commanded  forces  in  the  x  direction  the  force  that  develops  in 
the  y  direction  is  very  small  and  probably  results  from  slight 
errors  in  the  alignment  of  the  constraint  surface  and  the 
manipulator.  If  the  rigid  constraint  is  not  perpendicular  to 
the  direction  of  the  commanded  force,  the  motion  of  the  end 
effector  results  in  a  deflection  of  the  end  effector  in  the 
other  directions  and  a  resulting  frictional  forces  in  those 
directions  (Figures  4.9  and  4.11).  For  a  commanded  force  in 
the  x  direction  and  low  friction  between  the  end  effector  and 
the  constraint,  force  errors  in  the  y  direction  will  result  in 
motion  of  the  manipulator  at  rate  proportional  to  the  force 
error  in  that  direction.  The  end  effector  slides  along  the 
constraint  surface  with  a  constant  reaction  force  between  the 
end  effector  and  the  surface  of  the  constraint.  The  same 
explanation  describes  the  response  observed  in  the  z 
direction.  However,  much  larger  forces  are  expected  to 
develop  in  the  z  direction  due  to  the  higher  stiffness  of  the 
end  effector  in  that  direction. 

Misalignment  of  the  end  effector's  coordinate  frame 
and  the  manipulator's  tool  frame  may  also  contribute  to  this 
problem.  For  example,  if  the  x  and  y  coordinate  frames  of  the 
end  effector  are  not  properly  aligned  with  the  x  and  y 
coordinates  of  the  manipulator  tool  frame  motion  of  the 
manipulator  along  the  tool  frame's  x  axis  when  it  is  against 
a  constraint  will  result  in  a  deflection  of  the  end  effector 
and  measured  forces  in  both  the  x  and  y  directions.  This 
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results  in  force  errors  in  both  directions  and  motion  in  both 
directions  to  try  to  alleviate  the  associated  force  errors. 
This  is  probably  the  source  of  the  oscillations  observed  in 
Figure  4.11  in  the  y  direction.  This  contribution  to  the 
force  errors  can  be  minimized  with  accurate  alignment  of  the 
end  effector  and  manipulator's  tool  frame. 

The  steady  state  response  differs  from  that 
expected,  with  the  steady  state  errors  generally  being  lower 
than  anticipated  based  on  the  analysis  of  Chapters  II  and  III. 
Compare  the  Bode  plot  of  Figure  2 . 9  to  the  responses  shown  in 
Figures  4.8  through  4.14.  This  result  can  be  attributed  to 
the  discrete  nature  of  the  motion  commands  in  the  control 
program.  Each  motion  command  executed  within  the  control  loop 
can  be  looked  at  as  a  separate  motion  control  command.  The 
steady  state  error  will  depend  most  on  the  last  motion  command 
executed  and  the  actual  magnitude  of  the  force  command.  With 
a  larger  force  command  larger  forces  develop  at  the  end 
effector.  This  translates  to  a  larger  disturbance  on  each  of 
the  joint  servos  opposing  motion  in  the  desired  direction. 
The  results  support  this  discussion. 

The  response  is  more  rapid  and  a  smaller  steady 
state  error  results  as  the  value  of  3  increases (  Figures  4.8  - 
4.11,  4.12  and  4.13).  This  is  true  until  3  reaches  a  value 
of  four  where  unstable  behavior  begins  to  be  observed  (Figures 
4.11  and  4.14).  The  results  are  very  similar  for  commanded 
forces  in  both  the  x  and  y  directions,  with  the  system 
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approaching  unstable  behavior  for  values  of  (3  between  two  and 
four.  The  response  in  the  y  direction  is  more  rapid  than  in 
the  x  direction  due  to  the  lower  stiffness  of  the  end  effector 
in  the  y  direction.  Compare  the  response  rate  of  Figures  4.10 
through  4.11  to  those  of  Figures  4.12  through  4.14.  This 
lower  stiffness  in  the  y  direction  corresponds  to  a  higher 
force  error  gain  for  the  same  value  of  3.  The  stiffness  is 
almost  twice  as  high  in  the  x  direction,  resulting  in  a 
response  that  is  only  about  half  as  fast  as  the  response  in 
the  y  direction.  From  the  results  shown  in  Figures  4.9  and 
4.12,  one  can  see  that  the  steady  state  error  is  much  lower 
for  the  response  to  a  commanded  force  in  the  y  direction  with 
the  same  value  of  3.  This  tends  to  indicate  that  the  steady 
state  error  is  not  really  a  function  of  the  value  of  3,  but 
rather  is  dependent  on  the  stiffness  of  the  system,  including 
the  end  effector,  and  the  actual  control  system  gains.  This 
opposes  the  steady  state  error  of  the  linear  system  of 
Equation  2.17.  The  results  do  confirm  the  advantages  a  system 
with  lower  stiffness  has  in  controlling  forces. 

There  is  essentially  no  change  in  the  speed  of 
response  and  there  is  a  minimal  improvement  in  the  steady 
state  response  when  using  integral  control  in  the  individual 
joint  servo  controllers.  Compare  the  response  of  Figure  4.18 
to  that  of  Figure  4.19.  This  small  gain  in  performance  does 
not  justify  the  risk  of  overloading  the  individual  joint 
servos  when  integral  control  is  used. 
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Adding  integral  control  to  the  force  control 
algorithm  also  fails  to  confirm  any  significant  improvement  in 
the  steady  state  force  error.  Results  of  tests  for  which 
integral  control  to  the  force  error  is  added  did  indicate  that 
the  concept  works  but  the  problems  associated  with  control  of 
motion  in  free  space  renders  this  control  system  approach 
inappropriate  for  this  system.  If  the  actual  forces  at  the 
end  effector  are  known,  rather  than  the  force  errors  between 
the  end  effector  and  the  joystick,  an  adaptive  control 
algorithm  could  switch  on  or  off  the  integral  control  of  the 
force  error  depending  on  whether  contact  is  made  with  an 
obstruction  or  not.  This  approach  would  suit  control  of 
forces  with  a  stationary  constraint  well,  but  would  not  be 
suited  for  control  of  inertial  loads.  The  problem  of 
accelerating  the  payload  load  motion  rather  than  pure  rate 
control  would  once  again, result  in  this  situation. 
b.  Sinusoidal  Force  Input 

The  results  of  Figures  4.18  and  4.20  indicate  that 
response  magnitude  decreases  and  phase  lag  increases  with  the 
higher  frequency  input  as  expected  based  on  the  frequency 
response  of  the  single  DOF  analysis  of  Chapter  II.  The 
response  to  a  force  command  is  measured  for  only  two 
frequencies,  but  a  more  complete  frequency  response  for  a 
particular  set  of  control  gains  and  environmental  constraints 
can  be  obtained  by  measuring  the  response  for  a  complete 
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series  of  frequencies  and  plotting  the  results  in  the  form  of 
a  Bode  plot.  This  is  not  conducted  since  the  system  is 
expected  to  be  modified  by  replacing  the  joystick  and  end 
effector  with  devices  utilizing  Force  Sensing  Resistors  to 
measure  forces.  This  will  alter  the  response  of  the  system 
and  invalidate  specific  results  found  during  these  tests. 

Figure  4.21  shows  noticeable  oscillations  occurring  in 
the  forces  developed  in  both  the  y  and  z  directions  at 
approximately  the  same  frequency  as  the  commanded  force  in  the 
x  direction.  This  is  likely  due  to  the  errors  in  the 
alignment  of  the  coordinate  axes  of  the  end  effector  and 
manipulator  tool  frames  as  discussed  earlier.  The  magnitude 
of  the  oscillations  in  the  y  direction  are  relatively  low 
while  those  of  the  z  direction  are  much  higher.  This  is 
expected  due  to  the  variation  in  stiffness  of  the  end  effector 
between  these  directions  as  discussed  earlier.  Notice  that 
the  response  of  Figures  4.18  and  4.19  has  much  less  lag  and  a 
magnitude  almost  reaching  the  commanded  force  at  the  gain 
condition  of  3=2.  The  lag  is  even  more  reduced  for  3=4,  but 
the  system  is  approaching  unstable  behavior.  The  delays 
associated  with  program  computations  and  data  exchange  clearly 
reduce  the  stability  of  the  system. 

3 .  Task-oriented  Tests 

The  results  of  the  efforts  to  move  a  constrained 
object  around  through  contact  forces  provided  unanticipated 
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results.  The  motion  of  the  manipulator  actually  sliding 
around  the  obstruction  is  not  expected  but  is  easily 
explained.  When  the  contact  is  made  against  the  non-flat 
surface  of  the  obstruction  forces  develop  in  directions  other 
than  the  commanded  direction.  These  forces  can  be  resolved 
into  normal  and  tangential  components  relative  to  the 
obstructions  surface.  They  can  also  be  resolved  into  the 
three  directions  of  the  end  effector's  reference  frame.  Force 
error  signals  in  directions  other  than  the  commanded  force 
direction  develop,  causing  motion  control  commands  and 
movement  of  the  manipulator  to  reduce  these  force  errors. 

Several  things  can  limit  or  prevent  this  type  of 
response.  If  the  end  effector  contacts  the  constraint  in  a 
way  in  which  the  reaction  force  is  directly  in  line  with  the 
commanded  force,  no  force  signals  will  develop  at  the  end 
effector  in  another  direction  to  cause  this  lateral  motion 
around  the  constraint.  Also,  a  slight  misalignment  can  occur 
as  long  as  static  friction  between  the  end  effector  and  the 
constraint  is  large  enough  to  counteract  the  force  error 
tangent  to  the  constraint's  surface.  Another  way  to  prevent 
the  lateral  motion  is  to  reduce  the  control  gains  or  set  them 
equal  to  zero  in  the  directions  motion  is  not  desired  to 
prevent  this  lateral  motion.  This  has  the  potential  for 
allowing  high  forces  to  develop  in  these  directions. 

In  many  situations  this  accommodating  motion  is 
desired.  For  example,  in  the  assembly  task  of  inserting  a  peg 
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in  a  hole  the  lateral  motion  to  center  the  peg  in  the  hole  as 
it  is  inserted  is  desired  and  this  control  system  would 
automatically  perform  this  accommodating  motion.  Forces  in 
lateral  directions  the  peg  would  be  minimized. 

This  control  system  has  the  capability  to  be 
applied  to  do  such  tasks  as  washing  a  window  or  rotating  a 
crank,  tasks  normally  seen  performed  with  a  hybrid 
force/position  controller.  The  system  can  be  programmed  to 
maintain  a  specified  force  normal  to  the  window  and  then  the 
rate  commands  can  be  issued  using  the  joystick  to  move  the 
manipulator  along  the  surface  since  the  motion  is  not 
constrained  in  direction  parallel  to  the  surface. 

4.  Control  of  an  Inertial  Load  on  a  Flat  Surface 

The  friction  forces  between  the  disk  and  the  surface 
provide  limited  simulation  of  an  inertial  load  but  this 
oscillatory  motion  is  an  important  response  that  would  be 
expected  when  handling  a  purely  inertial  load.  Normally  the 
end  effector  itself  has  a  very  high  natural  frequency  and 
vibration  of  the  end  effector  is  of  small  magnitude.  This  has 
a  limited  effect  on  the  system  response.  In  this  situation, 
where  a  load  has  been  added  to  the  tip  of  the  end  effector, 
essentially,  the  frequency  of  oscillation  decreases 
considerably  while  the  magnitude  increases.  This  results  in 
an  oscillations  in  the  control  system  force  error  command 
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input  which  leads  to  oscillations  in  the  manipulator's  motion. 
This  can  result  in  an  unstable  response  of  the  system. 

A  major  factor  contributing  to  this  behavior  while 
controlling  an  inertial  load  is  the  stiffness  of  the 
manipulator  and  end  effector.  In  this  application  the 
stiffness  of  the  manipulator  is  set  by  the  PUMA  control  system 
and  is  considered  unchangeable.  This  limits  one  to  looking  at 
the  design  of  the  end  effector  to  prevent  this  undesired 
response.  High  mechanical  stiffness  of  the  end  effector  is 
desired  to  prevent  this  potentially  unstable  motion  with  an 
inertial  load.  This  raises  the  frequency  of  oscillation  and 
more  importantly  reduces  the  magnitude  of  the  oscillation. 
Including  some  type  of  damper  would  also  serve  to  limit  the 
amount  of  oscillation  which  occurs  when  controlling  an 
inertial  load.  This  need  for  high  stiffness  in  controlling  an 
inertial  load  contradicts  the  desire  for  low  stiffness  to 
provide  a  rapid,  stable  response.  A  trade-off  is  required  for 
unless  the  control  system  of  the  manipulator  itself  is  changed 
to  allow  for  variable  control  gains  to  change  the 
manipulator's  stiffness  for  an  optimum  response. 

B.  SYSTEM  STABILITY 

The  test  results  demonstrate  the  limited  stability  of  the 
system  and  the  effect  changing  the  control  gains  and  the 
environmental  constraints  has  on  stability.  It  is  clear  from 
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the  test  results  that  there  is  a  trade  off  between  the 
system's  speed  of  response  and  stability. 

1.  Stiffness  of  End  Effector  and  Environment 

The  stiffness  of  the  end  effector,  and  of  the 
manipulator,  greatly  influence  the  performance  and  stability 
of  the  system.  The  results  indicate  a  correlation  between  the 
value  of  3  and  system  stability  exists.  In  both  the  x  and  y 
directions  the  system  response  becomes  oscillatory  at  3=4  when 
subjected  to  a  step  input.  Oscillation  occurs  in  the  x 
direction  when  subjected  to  a  sinusoidal  input  at  this  same 
value  of  3.  See  Figures  4.11  and  4.14.  This  occurs  for  a 
rigid  environmental  constraint.  A  similar  correlation  can  be 
developed  for  the  effect  changes  in  environmental  stiffness 
has  on  stability.  Oscillation  appear  to  occur  at  the  same 
value  of  3  regardless  of  the  magnitude  of  the  actual  force 
command.  This  indicates  no  connection  between  the  magnitude 
of  the  input  and  the  system  stability. 

Tests  can  be  run  to  determine  the  limiting  value  of  3 
for  maintaining  stability  for  a  particular  constraint 
condition  and  an  adaptive  control  algorithm  be  developed  to 
adjust  the  program  force  error  gains  to  maintain  3  in  a  stable 
region.  This  requires  a  means  of  evaluating  the  constraint 
stiffness  in  real  time  during  manipulator  motion.  The 
existing  control  system  must  be  modified  to  provide  the 
ability  to  measure  and  utilize  the  actual  forces  acting  at  the 
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end  effector,  not  the  force  error  between  the  end  effector  and 
joystick.  With  knowledge  of  the  actual  forces,  the  stiffness 
of  the  environmental  constraint  can  be  continuously  calculated 
in  each  direction  within  the  control  algorithm.  This 
stiffness  can  then  be  correlated  to  a  satisfactory  control 
gain  for  maintaining  stability,  similar  to  what  is  performed 
in  the  compliance  controller  developed  in  [Ref.  9]. 

2 .  Computational  Delays 

Computational  delays  have  a  very  destabilizing  effect 
on  a  control  system.  The  delays  in  this  system  drives  an 
inherently  stable  response  of  a  system  with  no  delays  to 
unstable  behavior  in  some  situations.  Building  the  force 
override  controller  on  top  of  the  existing  PUMA  control  system 
greatly  limits  efforts  to  minimize  the  delay  time.  The 
performance  of  the  system  is  most  limited  by  the  structure  of 
the  continuous  path  motion  control  algorithm  of  the 
manipulator.  Being  a  position-based  control  system,  the 
PUMA'S  performance  is  worse  when  no  effort  is  made  to  ensure 
continuous  path  motion  is  maintained  while  the  manipulator  is 
moving  in  free  space.  The  additional  time  required  for 
acceleration  and  deceleration  of  the  manipulator  slows  down 
the  overall  performance  and  results  in  jerky  motion.  Similar 
operating  constraints  will  exist  for  other  positioned-based 
control  systems . 
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C.  COMPARISON  BETWEEN  SIMULATION  AND  ACTUAL  SYSTEM 

In  general,  the  response  of  the  simulation  results  are 
more  rapid  than  the  response  of  the  actual  system  for  a  step 
commanded  force.  Compare  the  simulation  responses  shown  in 
Figure  4.24  with  those  of  Figures  4.9,  4.10,  and  4.11  as  well 
as  those  of  Figure  4.25  with  Figures  4.12,  4.13,  and  4.14  and 
the  differences  in  the  rate  of  response  between  the  simulation 
and  the  actual  system  are  readily  apparent.  Comparing  Figures 
4.26  and  4.27  with  Figures  4.19  and  4.20,  this  is  also  the 
case  for  the  sinusoidal  responses,  but  not  nearly  as 
noticeable.  Several  factors  contribute  to  the  differences 
in  response.  The  end  effector  stiffness  use^  for  the 
simulation  are  only  estimates  of  the  actual  end  effector 
stiffness.  An  actual  end  effector  stiffness  much  lower  than 
the  estimated  stiffness  used  in  the  control  program  and  the 
simulation  would  explain  much  of  the  difference  between  the 
actual  results  and  the  simulation.  Additionally,  calibration 
errors  and  the  varying  and  the  lack  of  an  exact  speed  of 
motion,  as  discussed  above,  are  not  simulated  and  contribute 
to  the  discrepancies  between  the  simulation  and  actual 
results.  The  discrepancies  can  not  be  fully  explained  without 
better  knowledge  of  the  actual  system  parameters. 

The  simulation  results  depict  the  performance  of  the 
actual  system  effectively.  For  the  step  commanded  force  the 
response  of  the  system  as  it  begins  unstable  motion  is 
simulated  well,  as  shown  by  Figures  4.11  and  4.24.  The 
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simulation  response  of  Figure  4.26  does  not  depict  the 
unstable  motion  of  the  actual  system  in  Figure  4.18  for  the 
sinusoidal  force  input  nearly  as  well.  The  simulation 
algorithm  successfully  models  the  general  performance  of  the 
system  but  requires  improvements  to  more  accurately  predict 
the  actual  system  response.  The  algorithm  can  be  extended  to 
provide  a  six  DOF  model  of  the  manipulator.  In  this  model 
each  link  would  be  considered  as  a  dynamic  system  instead  of 
lumping  them  all  into  one  system  as  has  been  done  for  this 
simple  model.  This  would  require  much  more  knowledge  of  the 
manipulator's  characteristics  and  many  more  computations  to 
simulate  the  manipulator's  motion.  The  interaction  forces 
between  the  links  and  with  environmental  constraints  can  be 
modelled  to  varying  degrees  of  accuracy.  If  speeds  and 
accelerations  are  expected  to  be  very  low  Equation  2 . 9  can  be 
used  to  determine  the  reaction  forces  between  the  links  and  at 
the  end  effector.  If  high  speeds  or  high  accelerations  are 
expected  more  complex  equations  of  motion  must  be  developed  to 
model  the  manipulator's  dynamics.  Methods  for  calculating  and 
modelling  the  dynamics  are  discussed  in  [Ref.  28]  and 
[Ref.  29]. 
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V 


VI.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

•  Three  axis  unilateral  force  override  rate  control  has  been 
successfully  implemented  using  the  PUMA  560  manipulator. 

•  Stability  of  the  force  override  rate  control  system  is 
dependent  on  the  stiffness  of  the  manipulator,  the  end 
effector,  and  the  environmental  constraints. 

•  Steady  state  and  transient  performance  is  limited  due  to 
a  loss  of  stability  with  high  stiffness  and/or  high 
control  gains. 

•  The  system  stability  is  limited  due  to  the  use  of  constant 
feedback  gains  in  the  joint  servo  controllers. 

•  The  response  of  the  PUMA  560  force  override  rate  control 
system  performance  has  been  successfully  modelled  for  a 
single  DOF  using  a  simple  computer  model. 


B.  RECOMMENDATIONS 

•  Develop  and  construct  a  force  torque  sensor  and  associated 
hardware  utilizing  force  sensing  resistors  (FSR)  to 
replace  the  force  sensor  currently  used  for  the  joystick 
and  end  effector. 

•  Extend  the  control  system  to  control  not  only  forces  but 
also  torques,  ultimately  leading  to  six  DOF  force-torque 
override  of  linear  and  angular  rate  control  of  a 
manipulator. 

•  Develop  a  force-torque  display  to  assist  an  operator  in 
controlling  and  monitoring  system  conditions. 

•  Study  the  feasibility  of  using  an  adaptive  control 
algorithm  which  uses  real  time  force-torque  data  to  adjust 
system  gains  in  order  to  maintain  system  stability. 
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APPENDIX  A 


1 

2 

3 

4 

5 

6 

7 
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19 
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21 
22 

23 
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25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 


TYPE  "This  program  is  written  to  implement  force  control  of  the" 
TYPE  "PUMA  with  the  proper  hardware  installed." 

TYPE  "  " 

TYPE  "  " 

TYPE  "Do  you  want  to  use  integral  control  in  the  position" 

TYPE  "control  of  the  joint  servos  (Warning-excessive  control" 

TYPE  "may  develop  if  large  forces  or  high  stiffness  is  present)" 

10  PROMPT  "Enter  ’1’  for’YES’  and  ’2’  for  ’NO’", answer 
IF  answer  =  1  THEN 
dumy  =  1 
ELSE 

IF  answer  ==  2  THEN 
INTOFF  ALWAYS 
ELSE 

IF  answer  o  1  THEN 
GOTO  10 
END 
END 
END 
TYPE  "  " 

TYPE  "  " 

NONULL  ALWAYS 
ad  factor  =  (5/255)*(12/2.5) 
xamp.gain  =  1.80 
yamp.gain  =  2.75 
zamp.gain  =  0.6745 

TYPE  "Enter  the  factor  ’beta’  that  is  divided  by  the  estimated" 

TYPE  "environmental  and  end  effector  gain  (Ke)  to  give  the  force" 
TYPE  "error  gain  (kf)  in  the  x,  y,  and  z  directions." 

TYPE  "  " 

PROMPT  "?  ",  betax,  betay,  betaz 
xforce.gain  =  betax/ 1.205 
yforce.gain  =  betay/0.525 
zforce.gain  =  betaz/ 104.8 

TYPE  "Enter  the  desired  values  for  the  force  integral  control" 

TYPE  "gains  for  the  x,  y,  and  z  directions  respectively  if" 

TYPE  "integral  control  is  desired.  Enter  zeros  (0)  if  no" 

PROMPT  "integral  control  is  desired  ?  ”,xint.gain,  yint.gain,  zint.gain 
TYPE  "  " 

TYPE  "  " 

xforcecon  =  adfactor/xamp.gain 
yforcecon  =  adfactor/ yamp.gain 
zforcecon  =  adfactor/zamp.gain 
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44 

45 

46 

47 

48 

49 

50 

51 

52 

53 

54 

55 

56 

57 

58 

59 

60 
61 
62 

63 

64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

75 

76 

77 

78 

79 

80 
81 
82 

83 

84 

85 

86 

87 

88 

89 

90 

91 

92 

93 

94 


movetime  =  .4E-1 
maxspeed  =  462 
intxferr  =  0 
intyferr  =  0 
intzferr  =  0 

TYPE  "The  strain  gage  circuit  is  being  calibrated  now" 

TYPE  "Hit  ’RETURN’  when  the  joystick  and  endeffector  are" 
PROMPT  "clear  of  any  external  forces",  dumy 
SIGNAL  1,  2,  3,  4,  -5, -6, -7, -8 
FOR  i  =  1  TO  5 
delay  =  1 
END 

xcal  =  BITS(1001,  8) 

SIGNAL  -1,2,  3,  4,  -5,  -6,  -7,  -8 
FOR  i  =  1  TO  5 
delay  =  1 
END 

ycal  =  BITS(1001,  8) 

SIGNAL  1,  -2,3,  4,  -5,  -6,  -7,  -8 
FOR  i  =  1  TOS 
delay  =  1 
END 

zeal  =  BITS(1001,  8) 

TYPE  "  " 

TYPE  "Calibration  is  now  complete;  Proceeding." 

TYPE  "  " 

TYPE  "You  my  modify  the  system  to  establish  desired  force" 
PROMPT  "input.  Press  ’RETURN’  when  ready.", dumy 
50  SIGNAL  1,  2,  3,  4,  -5,  -6,  -7,  -8 
FOR  i  =  1  TO  5 
delay  =  1 
END 

xin  =  BITS(1001,  8) 

SIGNAL  -1,2,  3,  4,  -5,  -6,  -7,  -8 
FOR  i  =  1  TO  5 
delay  =  1 
END 

yin  =  BITS(1001,  8) 

SIGNAL  1,  -2,3,  4,  -5,  -6,  -7,  -8 
FOR  i  =  1  TO  5 
delay  =  1 
END 

zin  =  BITS(1001,  8) 

xerr  =  xcal- xin 

yerr  =  ycal -yin 

zerr  =  zeal- zin 

xferr  =  xerr*xforcecon 

yferr  =  yerr*forcecon 

zferr  =  -(zerr+14.3/12)*zforcecon 

xmove  =  xforce.gain*(xferr+intxferr*xint.gain) 

ymove  =  yforce.gain*(yferr+intyferr*yint.gain) 
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95  zmove  =  zforce.gain*(zferr+intzferr*zint.gain) 

96  distance  =  SQRT(SQR(xmove)+SQR(ymove)+SQR(zmove)) 

97  velocity  =  distance/movetime 

98  SPEED  =  100/SPEED(l)*100/maxspeed*velocity 

99  SET  delta  =  TRANS(xmove,  ymove,  zmove,  90,-90,0) 

100  HERE  initial 

101  MOVE  initial:delta 

102  intxferr  =  intzferr+xferr*movetime 

103  intyferr  =  intyferr+yferr*movetime 

104  intzferr  =  intzferr+zferr*movetime 

105  GOTO  50 
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APPENDIX  B 


'>»*>>»>>»»*>>>>>»>>>»>>>>»>»)>»»>»»>>»»»>)>»»>»>>>>>»>>»»>>>>>»> 


1  DOF  Force  Control  Simulation 
of  PUMA  560  Robot 
Larry  P.Ondrey 
Revised:  1/25/93 


>»>*>»>>*>*>»»>>>>>>>>>>>>>>>>>>*>>*>>>>>>>>>>>>>>>>>)>> 

’  List  of  Variables: 


inertia  :  Estimate  of  the  inertia  of  the  PUMA 
manipulator.(kg)  ’ 

ki.pos  :  Estimate  of  the  position  error  integral  gain  in  ’ 
the  servo  control  loop  of  the  PUMA, 
kp:  Estimate  of  the  position  error  gain  in  the  servo 
control  loop  of  the  PUMA  (N/m).  ’ 

damp  :  Estimate  of  the  damping  term  in  the  PUMA’s 
position  control  system;  assumed  to  provide 
critical  damping.  ’ 

kend  :  Estimate  of  the  end  effector’s  stiffness  (N/m).  ’ 

kf  :  Force  error  gain  used  in  the  PUMA  force  ’ 

control  algorithm  (m/N).  ’ 

ki. force  :  Force  error  integral  gain  used  in  the  PUMA 
force  control  algorithm  (m/N-sec).  ’ 

accelmax  :  Estimated  maximum  acceleration  of  the  PUMA 
endeffector  in  cartesian  space  (m/sec^).  ’ 

maxvel  :  Estimated  maximum  veloctiy  of  the  PUMA  end- 
effector  in  cartesian  space  (m/sec).  ’ 

rpos  :  Actual  position  of  robot  tool  mount  (m). 
veldes  :  Commanded  velocity  from  the  control  algorithm 
based  on  the  force  error  (m/sec).  ’ 

epos  :  Position  of  rigid  environment  (m). 
fdes  :  Commanded  force  from  joystick  (N). 
accel  :  Actual  accel.  of  the  PUMA’s  endeffector  (m/sec/'2).  ’ 
velocity  :  Actual  vel.  of  PUMA  endeffector  (m/sec), 
fact  :  Actual  force  being  applied  by  PUMA’s  endeffector 
on  the  environment  (N).  ’ 

time  :  Elapsed  time  of  system  conditions  from  the  the  ’ 
conditions  at  time=0  (sec).  ’ 

tstep  :  Time  increment  used  in  simulation  for  integration  ’ 
steps.  Based  on  loop  time  of  the  PUMA’s  individual  ’ 
servo  control  loops  -  .875  msec.  ’ 
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’  k  :  Sampling  counter  used  to  limit  the  number  of  ’ 

’  data  points  stored  in  the  data  file  -  Based  on 

*  the  actual  sampling  rate  of  the  data  aquisition  system.  ’ 

’  ferror  :  The  force  error  between  the  desired  force  and  the  ’ 

’  actual  force  at  a  particular  time.(N)  ’ 

’  nextmove  :  The  next  commanded  move  based  on  the  current 
’  force  error,  (m)  ’ 

’  desvel.next  :  The  next  commanded  velocity  based  on  the 
’  current  force  error.(m/sec)  ’ 

’  despos.next  :  The  next  position  the  control  system  is 
’  commanded  to  go  to.  (m)  ’ 

’  reqtime  :  The  estimate  of  the  time  that  the  current  move  is  ’ 
going  to  take  based  on  desired  speed  and  distance.’ 

’  (sec)  ’ 

’  cycles  :  The  number  of  cycles  the  PUMA’s  main  controller 
’  will  use  to  command  the  joint  servo  controllers  to  ’ 

’  perform  the  current  move.  ’ 

’  intposserr  :  The  integral  of  the  position  error;  used  in  the’ 

’  joint  servo  controllers  to  prevent  S.S  errors  ’ 

’  in  the  position  error  (m-sec)  ’ 

’  despos  :  The  current  commanded  position  that  the  PUMA 
’  main  controller  is  trying  to  achieve.(m)  ’ 

’  midpos  :  The  incremental  position  the  main  controller  ’ 

’  sends  to  the  individual  joint  controller  every  ’ 

’  28  msec.  ’ 

’  desvelocity  :  Estimate  of  the  current  commanded 
’  velocity  that  the  PUMA  main  controller  is 

’  trying  to  achieve.(m/sec)  ’ 

’  i  :  Counter  for  the  number  of  cycles  the  mian  controller  ’ 

’  has  gone  through  with  the  joint  servos.  ’ 

’  j  :  Counter  for  the  servo  controller  loop.  ’ 

’  poserr  :  The  current  position  error  used  in  the  servo 
’  controller  loop  to  control  the  commanded  ’ 

’  force.(m)  ’ 

’  fcomm  :  The  current  commanded  force  that  the  servo 
’  controller  sends  to  the  servo  motor. (N)  ’ 

’  ferrorint  :  The  integral  of  the  force  error.(N-m)  ’ 

»  > 

CLS 

PRINT 
PRINT 

PRINT  "This  program  simulates  the  dynamics  of  the  force  control 
PRINT 

PRINT  "system  of  the  PUMA  robot." 

PRINT 

OPEN  "c:\thesis\pumasim.dat"  FOR  OUTPUT  AS  #1 
’Initialize  system  "constants”: 
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inertia  =  5.38 
kp  =  85000 

damp  =  1!  *  2  *  SQR(kp  *  inertia) 
accelmax  =  4.12 
maxvel  =  .462 

> 

PRINT 

PRINT  "Enter  the  force  gain  (kf),  Force  integral  gain  (ki. force),” 
PRINT  "position  integral  gain  (ki.pos),  and  end  effector  " 

INPUT  "stiffness  (N/m)  ";  kf,  ki. force,  ki.pos,  kend 

PRINT 

PRINT 

kf  =  1  /  kf 

» 

Tntialize  system  varibles: 

* 

rpos  =  0 
midpos  =  0 
veldes  =  0 
epos  =  .001 

fdes  =  1.9  +  1.6  *  SIN(7.85  *  1.5) 

accel  =  0 

velocity  =  0 

fact  =  0 

time  =  0 

tstep  =  .875  /  1000 
k  =  0 

ferrorint  =  0 

> 

’  Store  initial  conditions  in  the  data  file. 

> 

PRINT  #1,  USING  "##.####,";  time;  fdes;  fact;  rpos 

> 

’  Establish  initial  control  variable  values. 

> 

ferror  =  fdes  -  fact 
nextmove  =  kf  *  ferror 
desvel.next  =  nextmove  /  .14 
IF  desvel.next  >  maxvel  THEN 

desvel.next  =  SGN(desvel.next)  *  maxvel 
END  IF 

despos.next  =  rpos  +  nextmove 

’  Begin  simulation  loop. 

» 

WHILE  time  <  6.5 
despos  =  despos.next 
desvei  =  desvel.next 
reqtime  =  ABSfnextmove  /  desvei) 
cycles  =  INTfreqtime  /  .028) 

IF  cycles  <  2  THEN 
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cycles  =  2 
END  IF 

FOR  i  =  1  TO  cycles 
intposerr  =  0 

midpos  =  rpos  +  desvel  *  .028 

j 

FOR  j  =  1  TO  32 
k  *  k  +  1 

time  =  time  +  tstep 

fdes  =  1.9  +  1.6  *  SIN (7. 8  *  (time  +  1.5)) 

poserr  =  midpos  -  rpos 

fcomm  =  kp  *  poserr  +  ki.pos  *  intposerr 

accel  =  1  /  inertia  *  (fcomm  -  fact)  -  velocity  *  damp  /  inertia 
IF  ABS(accel)  >  accelmax  THEN 
accel  =  SGN(accel)  *  accelmax 
END  IF 

’  Integration  steps 

> 

intposerr  =  intposerr  +  poserr  *  tstep 
rpos  =  rpos  +  velocity  *  tstep 
velocity  =  velocity  +  accel  *  tstep 
IF  velocity  >  desvel  THEN 
velocity  =  desvel 
END  IF 

IF  rpos  <  epos  THEN 
fact  =  0 
ELSE 

fact  =  kend  *  (rpos  -  epos) 

END  IF 

IF  k  =  13  THEN 
k  =  0 

PRINT  #1,  USING  "##.####,";  time;  fdes;  fact;  rpos 
END  IF 

NEXT  j 
IF  i  =  1  THEN 

ferror  =  fdes  -  fact 

ferrorint  =  ferrorint  +  terror  *  .028  *  cycles 
nextmove  =  kf  *  ferror  +  ki. force  *  ferrorint 
desvel.next  =  nextmove  /  .14 
IF  desvel.next  >  maxvel  THEN 

desvel.next  =  SGN(desvel.next)  *  maxvel 
END  IF 

despos.next  =  rpos  +  nextmove 
END  IF 

I 

NEXT  i 

» 

WEND 
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CLOSE  #1 
BEEP 

PRINT  "Simulation  is  complete.  Data  is  stored  in" 

PRINT 

PRINT  "file  PUMASIM.dat." 
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